drivers/tpm/tpm2_tis_sandbox.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (c) 2018, Bootlin
   4  * Author: Miquel Raynal <[email protected]>
   5  */
   6
   7 #include <common.h>
   8 #include <dm.h>
   9 #include <tpm-v2.h>
  10 #include <asm/state.h>
  11 #include <asm/unaligned.h>
  12 #include <linux/bitops.h>
  13 #include <u-boot/crc.h>
  14 #include <u-boot/sha256.h>
  15 #include "sandbox_common.h"
  16
  17 /* Hierarchies */
  18 enum tpm2_hierarchy {
  19         TPM2_HIERARCHY_LOCKOUT = 0,
  20         TPM2_HIERARCHY_ENDORSEMENT,
  21         TPM2_HIERARCHY_PLATFORM,
  22         TPM2_HIERARCHY_NB,
  23 };
  24
  25 /* Subset of supported capabilities */
  26 enum tpm2_capability {
  27         TPM_CAP_TPM_PROPERTIES = 0x6,
  28 };
  29
  30 /* Subset of supported properties */
  31 #define TPM2_PROPERTIES_OFFSET 0x0000020E
  32
  33 enum tpm2_cap_tpm_property {
  34         TPM2_FAIL_COUNTER = 0,
  35         TPM2_PROP_MAX_TRIES,
  36         TPM2_RECOVERY_TIME,
  37         TPM2_LOCKOUT_RECOVERY,
  38         TPM2_PROPERTY_NB,
  39 };
  40
  41 #define SANDBOX_TPM_PCR_NB 1
  42
  43 /*
  44  * Information about our TPM emulation. This is preserved in the sandbox
  45  * state file if enabled.
  46  *
  47  * @valid: true if this is valid (only used in s_state)
  48  * @init_done: true if open() has been called
  49  * @startup_done: true if TPM2_CC_STARTUP has been processed
  50  * @tests_done: true if TPM2_CC_SELF_TEST has be processed
  51  * @pw: TPM password per hierarchy
  52  * @pw_sz: Size of each password in bytes
  53  * @properties: TPM properties
  54  * @pcr: TPM Platform Configuration Registers. Each of these holds a hash and
  55  *      can be 'extended' a number of times, meaning another hash is added into
  56  *      its value (initial value all zeroes)
  57  * @pcr_extensions: Number of times each PCR has been extended (starts at 0)
  58  * @nvdata: non-volatile data, used to store important things for the platform
  59  */
  60 struct sandbox_tpm2 {
  61         bool valid;
  62         /* TPM internal states */
  63         bool init_done;
  64         bool startup_done;
  65         bool tests_done;
  66         char pw[TPM2_HIERARCHY_NB][TPM2_DIGEST_LEN + 1];
  67         int pw_sz[TPM2_HIERARCHY_NB];
  68         u32 properties[TPM2_PROPERTY_NB];
  69         u8 pcr[SANDBOX_TPM_PCR_NB][TPM2_DIGEST_LEN];
  70         u32 pcr_extensions[SANDBOX_TPM_PCR_NB];
  71         struct nvdata_state nvdata[NV_SEQ_COUNT];
  72 };
  73
  74 static struct sandbox_tpm2 s_state, *g_state;
  75
  76 /**
  77  * sandbox_tpm2_read_state() - read the sandbox EC state from the state file
  78  *
  79  * If data is available, then blob and node will provide access to it. If
  80  * not this function sets up an empty TPM.
  81  *
  82  * @blob: Pointer to device tree blob, or NULL if no data to read
  83  * @node: Node offset to read from
  84  */
  85 static int sandbox_tpm2_read_state(const void *blob, int node)
  86 {
  87         struct sandbox_tpm2 *state = &s_state;
  88         char prop_name[20];
  89         const char *prop;
  90         int len;
  91         int i;
  92
  93         if (!blob)
  94                 return 0;
  95         state->tests_done = fdtdec_get_int(blob, node, "tests-done", 0);
  96
  97         for (i = 0; i < TPM2_HIERARCHY_NB; i++) {
  98                 snprintf(prop_name, sizeof(prop_name), "pw%d", i);
  99
 100                 prop = fdt_getprop(blob, node, prop_name, &len);
 101                 if (len > TPM2_DIGEST_LEN)
 102                         return log_msg_ret("pw", -E2BIG);
 103                 if (prop) {
 104                         memcpy(state->pw[i], prop, len);
 105                         state->pw_sz[i] = len;
 106                 }
 107         }
 108
 109         for (i = 0; i < TPM2_PROPERTY_NB; i++) {
 110                 snprintf(prop_name, sizeof(prop_name), "properties%d", i);
 111                 state->properties[i] = fdtdec_get_uint(blob, node, prop_name,
 112                                                        0);
 113         }
 114
 115         for (i = 0; i < SANDBOX_TPM_PCR_NB; i++) {
 116                 int subnode;
 117
 118                 snprintf(prop_name, sizeof(prop_name), "pcr%d", i);
 119                 subnode = fdt_subnode_offset(blob, node, prop_name);
 120                 if (subnode < 0)
 121                         continue;
 122                 prop = fdt_getprop(blob, subnode, "value", &len);
 123                 if (len != TPM2_DIGEST_LEN)
 124                         return log_msg_ret("pcr", -E2BIG);
 125                 memcpy(state->pcr[i], prop, TPM2_DIGEST_LEN);
 126                 state->pcr_extensions[i] = fdtdec_get_uint(blob, subnode,
 127                                                            "extensions", 0);
 128         }
 129
 130         for (i = 0; i < NV_SEQ_COUNT; i++) {
 131                 struct nvdata_state *nvd = &state->nvdata[i];
 132
 133                 sprintf(prop_name, "nvdata%d", i);
 134                 prop = fdt_getprop(blob, node, prop_name, &len);
 135                 if (len > NV_DATA_SIZE)
 136                         return log_msg_ret("nvd", -E2BIG);
 137                 if (prop) {
 138                         memcpy(nvd->data, prop, len);
 139                         nvd->length = len;
 140                         nvd->present = true;
 141                 }
 142         }
 143         s_state.valid = true;
 144
 145         return 0;
 146 }
 147
 148 /**
 149  * sandbox_tpm2_write_state() - Write out our state to the state file
 150  *
 151  * The caller will ensure that there is a node ready for the state. The node
 152  * may already contain the old state, in which case it is overridden.
 153  *
 154  * @blob: Device tree blob holding state
 155  * @node: Node to write our state into
 156  */
 157 static int sandbox_tpm2_write_state(void *blob, int node)
 158 {
 159         const struct sandbox_tpm2 *state = g_state;
 160         char prop_name[20];
 161         int i;
 162
 163         if (!state)
 164                 return 0;
 165
 166         /*
 167          * We are guaranteed enough space to write basic properties. This is
 168          * SANDBOX_STATE_MIN_SPACE.
 169          *
 170          * We could use fdt_add_subnode() to put each set of data in its
 171          * own node - perhaps useful if we add access information to each.
 172          */
 173         fdt_setprop_u32(blob, node, "tests-done", state->tests_done);
 174
 175         for (i = 0; i < TPM2_HIERARCHY_NB; i++) {
 176                 if (state->pw_sz[i]) {
 177                         snprintf(prop_name, sizeof(prop_name), "pw%d", i);
 178                         fdt_setprop(blob, node, prop_name, state->pw[i],
 179                                     state->pw_sz[i]);
 180                 }
 181         }
 182
 183         for (i = 0; i < TPM2_PROPERTY_NB; i++) {
 184                 snprintf(prop_name, sizeof(prop_name), "properties%d", i);
 185                 fdt_setprop_u32(blob, node, prop_name, state->properties[i]);
 186         }
 187
 188         for (i = 0; i < SANDBOX_TPM_PCR_NB; i++) {
 189                 int subnode;
 190
 191                 snprintf(prop_name, sizeof(prop_name), "pcr%d", i);
 192                 subnode = fdt_add_subnode(blob, node, prop_name);
 193                 fdt_setprop(blob, subnode, "value", state->pcr[i],
 194                             TPM2_DIGEST_LEN);
 195                 fdt_setprop_u32(blob, subnode, "extensions",
 196                                 state->pcr_extensions[i]);
 197         }
 198
 199         for (i = 0; i < NV_SEQ_COUNT; i++) {
 200                 const struct nvdata_state *nvd = &state->nvdata[i];
 201
 202                 if (nvd->present) {
 203                         snprintf(prop_name, sizeof(prop_name), "nvdata%d", i);
 204                         fdt_setprop(blob, node, prop_name, nvd->data,
 205                                     nvd->length);
 206                 }
 207         }
 208
 209         return 0;
 210 }
 211
 212 SANDBOX_STATE_IO(sandbox_tpm2, "sandbox,tpm2", sandbox_tpm2_read_state,
 213                  sandbox_tpm2_write_state);
 214
 215 /*
 216  * Check the tag validity depending on the command (authentication required or
 217  * not). If authentication is required, check it is valid. Update the auth
 218  * pointer to point to the next chunk of data to process if needed.
 219  */
 220 static int sandbox_tpm2_check_session(struct udevice *dev, u32 command, u16 tag,
 221                                       const u8 **auth,
 222                                       enum tpm2_hierarchy *hierarchy)
 223 {
 224         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 225         u32 handle, auth_sz, session_handle;
 226         u16 nonce_sz, pw_sz;
 227         const char *pw;
 228
 229         switch (command) {
 230         case TPM2_CC_STARTUP:
 231         case TPM2_CC_SELF_TEST:
 232         case TPM2_CC_GET_CAPABILITY:
 233         case TPM2_CC_PCR_READ:
 234                 if (tag != TPM2_ST_NO_SESSIONS) {
 235                         printf("No session required for command 0x%x\n",
 236                                command);
 237                         return TPM2_RC_BAD_TAG;
 238                 }
 239
 240                 return 0;
 241
 242         case TPM2_CC_CLEAR:
 243         case TPM2_CC_HIERCHANGEAUTH:
 244         case TPM2_CC_DAM_RESET:
 245         case TPM2_CC_DAM_PARAMETERS:
 246         case TPM2_CC_PCR_EXTEND:
 247         case TPM2_CC_NV_READ:
 248         case TPM2_CC_NV_WRITE:
 249         case TPM2_CC_NV_WRITELOCK:
 250         case TPM2_CC_NV_DEFINE_SPACE:
 251                 if (tag != TPM2_ST_SESSIONS) {
 252                         printf("Session required for command 0x%x\n", command);
 253                         return TPM2_RC_AUTH_CONTEXT;
 254                 }
 255
 256                 handle = get_unaligned_be32(*auth);
 257                 *auth += sizeof(handle);
 258
 259                 /*
 260                  * PCR_Extend had a different protection mechanism and does not
 261                  * use the same standards as other commands.
 262                  */
 263                 if (command == TPM2_CC_PCR_EXTEND)
 264                         break;
 265
 266                 switch (handle) {
 267                 case TPM2_RH_LOCKOUT:
 268                         *hierarchy = TPM2_HIERARCHY_LOCKOUT;
 269                         break;
 270                 case TPM2_RH_ENDORSEMENT:
 271                         if (command == TPM2_CC_CLEAR) {
 272                                 printf("Endorsement hierarchy unsupported\n");
 273                                 return TPM2_RC_AUTH_MISSING;
 274                         }
 275                         *hierarchy = TPM2_HIERARCHY_ENDORSEMENT;
 276                         break;
 277                 case TPM2_RH_PLATFORM:
 278                         *hierarchy = TPM2_HIERARCHY_PLATFORM;
 279                         if (command == TPM2_CC_NV_READ ||
 280                             command == TPM2_CC_NV_WRITE ||
 281                             command == TPM2_CC_NV_WRITELOCK)
 282                                 *auth += sizeof(u32);
 283                         break;
 284                 default:
 285                         printf("Wrong handle 0x%x\n", handle);
 286                         return TPM2_RC_VALUE;
 287                 }
 288
 289                 break;
 290
 291         default:
 292                 printf("Command code not recognized: 0x%x\n", command);
 293                 return TPM2_RC_COMMAND_CODE;
 294         }
 295
 296         auth_sz = get_unaligned_be32(*auth);
 297         *auth += sizeof(auth_sz);
 298
 299         session_handle = get_unaligned_be32(*auth);
 300         *auth += sizeof(session_handle);
 301         if (session_handle != TPM2_RS_PW) {
 302                 printf("Wrong session handle 0x%x\n", session_handle);
 303                 return TPM2_RC_VALUE;
 304         }
 305
 306         nonce_sz = get_unaligned_be16(*auth);
 307         *auth += sizeof(nonce_sz);
 308         if (nonce_sz) {
 309                 printf("Nonces not supported in Sandbox, aborting\n");
 310                 return TPM2_RC_HANDLE;
 311         }
 312
 313         /* Ignore attributes */
 314         *auth += sizeof(u8);
 315
 316         pw_sz = get_unaligned_be16(*auth);
 317         *auth += sizeof(pw_sz);
 318         if (auth_sz != (9 + nonce_sz + pw_sz)) {
 319                 printf("Authentication size (%d) do not match %d\n",
 320                        auth_sz, 9 + nonce_sz + pw_sz);
 321                 return TPM2_RC_SIZE;
 322         }
 323
 324         /* No passwork is acceptable */
 325         if (!pw_sz && !tpm->pw_sz[*hierarchy])
 326                 return TPM2_RC_SUCCESS;
 327
 328         /* Password is too long */
 329         if (pw_sz > TPM2_DIGEST_LEN) {
 330                 printf("Password should not be more than %dB\n",
 331                        TPM2_DIGEST_LEN);
 332                 return TPM2_RC_AUTHSIZE;
 333         }
 334
 335         pw = (const char *)*auth;
 336         *auth += pw_sz;
 337
 338         /* Password is wrong */
 339         if (pw_sz != tpm->pw_sz[*hierarchy] ||
 340             strncmp(pw, tpm->pw[*hierarchy], tpm->pw_sz[*hierarchy])) {
 341                 printf("Authentication failed: wrong password.\n");
 342                 return TPM2_RC_BAD_AUTH;
 343         }
 344
 345         return TPM2_RC_SUCCESS;
 346 }
 347
 348 static int sandbox_tpm2_check_readyness(struct udevice *dev, int command)
 349 {
 350         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 351
 352         switch (command) {
 353         case TPM2_CC_STARTUP:
 354                 if (!tpm->init_done || tpm->startup_done)
 355                         return TPM2_RC_INITIALIZE;
 356
 357                 break;
 358         case TPM2_CC_GET_CAPABILITY:
 359                 if (!tpm->init_done || !tpm->startup_done)
 360                         return TPM2_RC_INITIALIZE;
 361
 362                 break;
 363         case TPM2_CC_SELF_TEST:
 364                 if (!tpm->startup_done)
 365                         return TPM2_RC_INITIALIZE;
 366
 367                 break;
 368         default:
 369                 if (!tpm->tests_done)
 370                         return TPM2_RC_NEEDS_TEST;
 371
 372                 break;
 373         }
 374
 375         return 0;
 376 }
 377
 378 static int sandbox_tpm2_fill_buf(u8 *recv, size_t *recv_len, u16 tag, u32 rc)
 379 {
 380         *recv_len = sizeof(tag) + sizeof(u32) + sizeof(rc);
 381
 382         /* Write tag */
 383         put_unaligned_be16(tag, recv);
 384         recv += sizeof(tag);
 385
 386         /* Write length */
 387         put_unaligned_be32(*recv_len, recv);
 388         recv += sizeof(u32);
 389
 390         /* Write return code */
 391         put_unaligned_be32(rc, recv);
 392         recv += sizeof(rc);
 393
 394         /* Add trailing \0 */
 395         *recv = '\0';
 396
 397         return 0;
 398 }
 399
 400 static int sandbox_tpm2_extend(struct udevice *dev, int pcr_index,
 401                                const u8 *extension)
 402 {
 403         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 404         sha256_context ctx;
 405
 406         /* Zero the PCR if this is the first use */
 407         if (!tpm->pcr_extensions[pcr_index])
 408                 memset(tpm->pcr[pcr_index], '\0', TPM2_DIGEST_LEN);
 409
 410         sha256_starts(&ctx);
 411         sha256_update(&ctx, tpm->pcr[pcr_index], TPM2_DIGEST_LEN);
 412         sha256_update(&ctx, extension, TPM2_DIGEST_LEN);
 413         sha256_finish(&ctx, tpm->pcr[pcr_index]);
 414
 415         tpm->pcr_extensions[pcr_index]++;
 416
 417         return 0;
 418 };
 419
 420 static int sandbox_tpm2_xfer(struct udevice *dev, const u8 *sendbuf,
 421                              size_t send_size, u8 *recvbuf,
 422                              size_t *recv_len)
 423 {
 424         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 425         enum tpm2_hierarchy hierarchy = 0;
 426         const u8 *sent = sendbuf;
 427         u8 *recv = recvbuf;
 428         u32 length, command, rc = 0;
 429         u16 tag, mode, new_pw_sz;
 430         u8 yes_no;
 431         int i, j;
 432
 433         /* TPM2_GetProperty */
 434         u32 capability, property, property_count;
 435
 436         /* TPM2_PCR_Read/Extend variables */
 437         int pcr_index = 0;
 438         u64 pcr_map = 0;
 439         u32 selections, pcr_nb;
 440         u16 alg;
 441         u8 pcr_array_sz;
 442
 443         tag = get_unaligned_be16(sent);
 444         sent += sizeof(tag);
 445
 446         length = get_unaligned_be32(sent);
 447         sent += sizeof(length);
 448         if (length != send_size) {
 449                 printf("TPM2: Unmatching length, received: %zd, expected: %d\n",
 450                        send_size, length);
 451                 rc = TPM2_RC_SIZE;
 452                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 453                 return 0;
 454         }
 455
 456         command = get_unaligned_be32(sent);
 457         sent += sizeof(command);
 458         rc = sandbox_tpm2_check_readyness(dev, command);
 459         if (rc) {
 460                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 461                 return 0;
 462         }
 463
 464         rc = sandbox_tpm2_check_session(dev, command, tag, &sent, &hierarchy);
 465         if (rc) {
 466                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 467                 return 0;
 468         }
 469
 470         switch (command) {
 471         case TPM2_CC_STARTUP:
 472                 mode = get_unaligned_be16(sent);
 473                 sent += sizeof(mode);
 474                 switch (mode) {
 475                 case TPM2_SU_CLEAR:
 476                 case TPM2_SU_STATE:
 477                         break;
 478                 default:
 479                         rc = TPM2_RC_VALUE;
 480                 }
 481
 482                 tpm->startup_done = true;
 483
 484                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 485                 break;
 486
 487         case TPM2_CC_SELF_TEST:
 488                 yes_no = *sent;
 489                 sent += sizeof(yes_no);
 490                 switch (yes_no) {
 491                 case TPMI_YES:
 492                 case TPMI_NO:
 493                         break;
 494                 default:
 495                         rc = TPM2_RC_VALUE;
 496                 }
 497
 498                 tpm->tests_done = true;
 499
 500                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 501                 break;
 502
 503         case TPM2_CC_CLEAR:
 504                 /* Reset this hierarchy password */
 505                 tpm->pw_sz[hierarchy] = 0;
 506
 507                 /* Reset all password if thisis the PLATFORM hierarchy */
 508                 if (hierarchy == TPM2_HIERARCHY_PLATFORM)
 509                         for (i = 0; i < TPM2_HIERARCHY_NB; i++)
 510                                 tpm->pw_sz[i] = 0;
 511
 512                 /* Reset the properties */
 513                 for (i = 0; i < TPM2_PROPERTY_NB; i++)
 514                         tpm->properties[i] = 0;
 515
 516                 /* Reset the PCRs and their number of extensions */
 517                 for (i = 0; i < SANDBOX_TPM_PCR_NB; i++) {
 518                         tpm->pcr_extensions[i] = 0;
 519                         for (j = 0; j < TPM2_DIGEST_LEN; j++)
 520                                 tpm->pcr[i][j] = 0;
 521                 }
 522
 523                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 524                 break;
 525
 526         case TPM2_CC_HIERCHANGEAUTH:
 527                 new_pw_sz = get_unaligned_be16(sent);
 528                 sent += sizeof(new_pw_sz);
 529                 if (new_pw_sz > TPM2_DIGEST_LEN) {
 530                         rc = TPM2_RC_SIZE;
 531                 } else if (new_pw_sz) {
 532                         tpm->pw_sz[hierarchy] = new_pw_sz;
 533                         memcpy(tpm->pw[hierarchy], sent, new_pw_sz);
 534                         sent += new_pw_sz;
 535                 }
 536
 537                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 538                 break;
 539
 540         case TPM2_CC_GET_CAPABILITY:
 541                 capability = get_unaligned_be32(sent);
 542                 sent += sizeof(capability);
 543                 if (capability != TPM_CAP_TPM_PROPERTIES) {
 544                         printf("Sandbox TPM only support TPM_CAPABILITIES\n");
 545                         return TPM2_RC_HANDLE;
 546                 }
 547
 548                 property = get_unaligned_be32(sent);
 549                 sent += sizeof(property);
 550                 property -= TPM2_PROPERTIES_OFFSET;
 551
 552                 property_count = get_unaligned_be32(sent);
 553                 sent += sizeof(property_count);
 554                 if (!property_count ||
 555                     property + property_count > TPM2_PROPERTY_NB) {
 556                         rc = TPM2_RC_HANDLE;
 557                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 558                 }
 559
 560                 /* Write tag */
 561                 put_unaligned_be16(tag, recv);
 562                 recv += sizeof(tag);
 563
 564                 /* Ignore length for now */
 565                 recv += sizeof(u32);
 566
 567                 /* Write return code */
 568                 put_unaligned_be32(rc, recv);
 569                 recv += sizeof(rc);
 570
 571                 /* Tell there is more data to read */
 572                 *recv = TPMI_YES;
 573                 recv += sizeof(yes_no);
 574
 575                 /* Repeat the capability */
 576                 put_unaligned_be32(capability, recv);
 577                 recv += sizeof(capability);
 578
 579                 /* Give the number of properties that follow */
 580                 put_unaligned_be32(property_count, recv);
 581                 recv += sizeof(property_count);
 582
 583                 /* Fill with the properties */
 584                 for (i = 0; i < property_count; i++) {
 585                         put_unaligned_be32(TPM2_PROPERTIES_OFFSET + property +
 586                                            i, recv);
 587                         recv += sizeof(property);
 588                         put_unaligned_be32(tpm->properties[property + i],
 589                                            recv);
 590                         recv += sizeof(property);
 591                 }
 592
 593                 /* Add trailing \0 */
 594                 *recv = '\0';
 595
 596                 /* Write response length */
 597                 *recv_len = recv - recvbuf;
 598                 put_unaligned_be32(*recv_len, recvbuf + sizeof(tag));
 599
 600                 break;
 601
 602         case TPM2_CC_DAM_PARAMETERS:
 603                 tpm->properties[TPM2_PROP_MAX_TRIES] = get_unaligned_be32(sent);
 604                 sent += sizeof(*tpm->properties);
 605                 tpm->properties[TPM2_RECOVERY_TIME] = get_unaligned_be32(sent);
 606                 sent += sizeof(*tpm->properties);
 607                 tpm->properties[TPM2_LOCKOUT_RECOVERY] = get_unaligned_be32(sent);
 608                 sent += sizeof(*tpm->properties);
 609
 610                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 611                 break;
 612
 613         case TPM2_CC_PCR_READ:
 614                 selections = get_unaligned_be32(sent);
 615                 sent += sizeof(selections);
 616                 if (selections != 1) {
 617                         printf("Sandbox cannot handle more than one PCR\n");
 618                         rc = TPM2_RC_VALUE;
 619                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 620                 }
 621
 622                 alg = get_unaligned_be16(sent);
 623                 sent += sizeof(alg);
 624                 if (alg != TPM2_ALG_SHA256) {
 625                         printf("Sandbox TPM only handle SHA256 algorithm\n");
 626                         rc = TPM2_RC_VALUE;
 627                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 628                 }
 629
 630                 pcr_array_sz = *sent;
 631                 sent += sizeof(pcr_array_sz);
 632                 if (!pcr_array_sz || pcr_array_sz > 8) {
 633                         printf("Sandbox TPM cannot handle so much PCRs\n");
 634                         rc = TPM2_RC_VALUE;
 635                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 636                 }
 637
 638                 for (i = 0; i < pcr_array_sz; i++)
 639                         pcr_map += (u64)sent[i] << (i * 8);
 640
 641                 if (!pcr_map) {
 642                         printf("Empty PCR map\n");
 643                         rc = TPM2_RC_VALUE;
 644                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 645                 }
 646
 647                 for (i = 0; i < SANDBOX_TPM_PCR_NB; i++)
 648                         if (pcr_map & BIT(i))
 649                                 pcr_index = i;
 650
 651                 if (pcr_index >= SANDBOX_TPM_PCR_NB) {
 652                         printf("Invalid index %d, sandbox TPM handles up to %d PCR(s)\n",
 653                                pcr_index, SANDBOX_TPM_PCR_NB);
 654                         rc = TPM2_RC_VALUE;
 655                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 656                 }
 657
 658                 /* Write tag */
 659                 put_unaligned_be16(tag, recv);
 660                 recv += sizeof(tag);
 661
 662                 /* Ignore length for now */
 663                 recv += sizeof(u32);
 664
 665                 /* Write return code */
 666                 put_unaligned_be32(rc, recv);
 667                 recv += sizeof(rc);
 668
 669                 /* Number of extensions */
 670                 put_unaligned_be32(tpm->pcr_extensions[pcr_index], recv);
 671                 recv += sizeof(u32);
 672
 673                 /* Copy the PCR */
 674                 memcpy(recv, tpm->pcr[pcr_index], TPM2_DIGEST_LEN);
 675                 recv += TPM2_DIGEST_LEN;
 676
 677                 /* Add trailing \0 */
 678                 *recv = '\0';
 679
 680                 /* Write response length */
 681                 *recv_len = recv - recvbuf;
 682                 put_unaligned_be32(*recv_len, recvbuf + sizeof(tag));
 683
 684                 break;
 685
 686         case TPM2_CC_PCR_EXTEND:
 687                 /* Get the PCR index */
 688                 pcr_index = get_unaligned_be32(sendbuf + sizeof(tag) +
 689                                                sizeof(length) +
 690                                                sizeof(command));
 691                 if (pcr_index >= SANDBOX_TPM_PCR_NB) {
 692                         printf("Invalid index %d, sandbox TPM handles up to %d PCR(s)\n",
 693                                pcr_index, SANDBOX_TPM_PCR_NB);
 694                         rc = TPM2_RC_VALUE;
 695                 }
 696
 697                 /* Check the number of hashes */
 698                 pcr_nb = get_unaligned_be32(sent);
 699                 sent += sizeof(pcr_nb);
 700                 if (pcr_nb != 1) {
 701                         printf("Sandbox cannot handle more than one PCR\n");
 702                         rc = TPM2_RC_VALUE;
 703                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 704                 }
 705
 706                 /* Check the hash algorithm */
 707                 alg = get_unaligned_be16(sent);
 708                 sent += sizeof(alg);
 709                 if (alg != TPM2_ALG_SHA256) {
 710                         printf("Sandbox TPM only handle SHA256 algorithm\n");
 711                         rc = TPM2_RC_VALUE;
 712                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 713                 }
 714
 715                 /* Extend the PCR */
 716                 rc = sandbox_tpm2_extend(dev, pcr_index, sent);
 717
 718                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 719                 break;
 720
 721         case TPM2_CC_NV_READ: {
 722                 int index, seq;
 723
 724                 index = get_unaligned_be32(sendbuf + TPM2_HDR_LEN + 4);
 725                 length = get_unaligned_be16(sent);
 726                 /* ignore offset */
 727                 seq = sb_tpm_index_to_seq(index);
 728                 if (seq < 0)
 729                         return log_msg_ret("index", -EINVAL);
 730                 printf("tpm: nvread index=%#02x, len=%#02x, seq=%#02x\n", index,
 731                        length, seq);
 732                 *recv_len = TPM2_HDR_LEN + 6 + length;
 733                 memset(recvbuf, '\0', *recv_len);
 734                 put_unaligned_be32(length, recvbuf + 2);
 735                 sb_tpm_read_data(tpm->nvdata, seq, recvbuf,
 736                                  TPM2_HDR_LEN + 4 + 2, length);
 737                 break;
 738         }
 739         case TPM2_CC_NV_WRITE: {
 740                 int index, seq;
 741
 742                 index = get_unaligned_be32(sendbuf + TPM2_HDR_LEN + 4);
 743                 length = get_unaligned_be16(sent);
 744                 sent += sizeof(u16);
 745
 746                 /* ignore offset */
 747                 seq = sb_tpm_index_to_seq(index);
 748                 if (seq < 0)
 749                         return log_msg_ret("index", -EINVAL);
 750                 printf("tpm: nvwrite index=%#02x, len=%#02x, seq=%#02x\n", index,
 751                        length, seq);
 752                 memcpy(&tpm->nvdata[seq].data, sent, length);
 753                 tpm->nvdata[seq].present = true;
 754                 *recv_len = TPM2_HDR_LEN + 2;
 755                 memset(recvbuf, '\0', *recv_len);
 756                 break;
 757         }
 758         case TPM2_CC_NV_DEFINE_SPACE: {
 759                 int policy_size, index, seq;
 760
 761                 policy_size = get_unaligned_be16(sent + 12);
 762                 index = get_unaligned_be32(sent + 2);
 763                 sent += 14 + policy_size;
 764                 length = get_unaligned_be16(sent);
 765                 seq = sb_tpm_index_to_seq(index);
 766                 if (seq < 0)
 767                         return -EINVAL;
 768                 printf("tpm: define_space index=%x, len=%x, seq=%x, policy_size=%x\n",
 769                        index, length, seq, policy_size);
 770                 sb_tpm_define_data(tpm->nvdata, seq, length);
 771                 *recv_len = 12;
 772                 memset(recvbuf, '\0', *recv_len);
 773                 break;
 774         }
 775         case TPM2_CC_NV_WRITELOCK:
 776                 *recv_len = 12;
 777                 memset(recvbuf, '\0', *recv_len);
 778                 break;
 779         default:
 780                 printf("TPM2 command %02x unknown in Sandbox\n", command);
 781                 rc = TPM2_RC_COMMAND_CODE;
 782                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 783         }
 784
 785         return 0;
 786 }
 787
 788 static int sandbox_tpm2_get_desc(struct udevice *dev, char *buf, int size)
 789 {
 790         if (size < 15)
 791                 return -ENOSPC;
 792
 793         return snprintf(buf, size, "Sandbox TPM2.x");
 794 }
 795
 796 static int sandbox_tpm2_open(struct udevice *dev)
 797 {
 798         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 799
 800         if (tpm->init_done)
 801                 return -EIO;
 802
 803         tpm->init_done = true;
 804
 805         return 0;
 806 }
 807
 808 static int sandbox_tpm2_probe(struct udevice *dev)
 809 {
 810         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 811         struct tpm_chip_priv *priv = dev_get_uclass_priv(dev);
 812
 813         /* Use the TPM v2 stack */
 814         priv->version = TPM_V2;
 815
 816         priv->pcr_count = 32;
 817         priv->pcr_select_min = 2;
 818
 819         if (s_state.valid)
 820                 memcpy(tpm, &s_state, sizeof(*tpm));
 821         g_state = tpm;
 822
 823         return 0;
 824 }
 825
 826 static int sandbox_tpm2_close(struct udevice *dev)
 827 {
 828         return 0;
 829 }
 830
 831 static const struct tpm_ops sandbox_tpm2_ops = {
 832         .open           = sandbox_tpm2_open,
 833         .close          = sandbox_tpm2_close,
 834         .get_desc       = sandbox_tpm2_get_desc,
 835         .xfer           = sandbox_tpm2_xfer,
 836 };
 837
 838 static const struct udevice_id sandbox_tpm2_ids[] = {
 839         { .compatible = "sandbox,tpm2" },
 840         { }
 841 };
 842
 843 U_BOOT_DRIVER(sandbox_tpm2) = {
 844         .name   = "sandbox_tpm2",
 845         .id     = UCLASS_TPM,
 846         .of_match = sandbox_tpm2_ids,
 847         .ops    = &sandbox_tpm2_ops,
 848         .probe  = sandbox_tpm2_probe,
 849         .priv_auto      = sizeof(struct sandbox_tpm2),
 850 };