drivers/tpm/tpm2_tis_sandbox.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Copyright (c) 2018, Bootlin
   4  * Author: Miquel Raynal <miquel.raynal@bootlin.com>
   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                 /* Skip this, since the startup may have happened in SPL
 370                  * if (!tpm->tests_done)
 371                  *    return TPM2_RC_NEEDS_TEST;
 372                  */
 373
 374                 break;
 375         }
 376
 377         return 0;
 378 }
 379
 380 static int sandbox_tpm2_fill_buf(u8 *recv, size_t *recv_len, u16 tag, u32 rc)
 381 {
 382         *recv_len = sizeof(tag) + sizeof(u32) + sizeof(rc);
 383
 384         /* Write tag */
 385         put_unaligned_be16(tag, recv);
 386         recv += sizeof(tag);
 387
 388         /* Write length */
 389         put_unaligned_be32(*recv_len, recv);
 390         recv += sizeof(u32);
 391
 392         /* Write return code */
 393         put_unaligned_be32(rc, recv);
 394         recv += sizeof(rc);
 395
 396         /* Add trailing \0 */
 397         *recv = '\0';
 398
 399         return 0;
 400 }
 401
 402 static int sandbox_tpm2_extend(struct udevice *dev, int pcr_index,
 403                                const u8 *extension)
 404 {
 405         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 406         sha256_context ctx;
 407
 408         /* Zero the PCR if this is the first use */
 409         if (!tpm->pcr_extensions[pcr_index])
 410                 memset(tpm->pcr[pcr_index], '\0', TPM2_DIGEST_LEN);
 411
 412         sha256_starts(&ctx);
 413         sha256_update(&ctx, tpm->pcr[pcr_index], TPM2_DIGEST_LEN);
 414         sha256_update(&ctx, extension, TPM2_DIGEST_LEN);
 415         sha256_finish(&ctx, tpm->pcr[pcr_index]);
 416
 417         tpm->pcr_extensions[pcr_index]++;
 418
 419         return 0;
 420 };
 421
 422 static int sandbox_tpm2_xfer(struct udevice *dev, const u8 *sendbuf,
 423                              size_t send_size, u8 *recvbuf,
 424                              size_t *recv_len)
 425 {
 426         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 427         enum tpm2_hierarchy hierarchy = 0;
 428         const u8 *sent = sendbuf;
 429         u8 *recv = recvbuf;
 430         u32 length, command, rc = 0;
 431         u16 tag, mode, new_pw_sz;
 432         u8 yes_no;
 433         int i, j;
 434
 435         /* TPM2_GetProperty */
 436         u32 capability, property, property_count;
 437
 438         /* TPM2_PCR_Read/Extend variables */
 439         int pcr_index = 0;
 440         u64 pcr_map = 0;
 441         u32 selections, pcr_nb;
 442         u16 alg;
 443         u8 pcr_array_sz;
 444
 445         tag = get_unaligned_be16(sent);
 446         sent += sizeof(tag);
 447
 448         length = get_unaligned_be32(sent);
 449         sent += sizeof(length);
 450         if (length != send_size) {
 451                 printf("TPM2: Unmatching length, received: %zd, expected: %d\n",
 452                        send_size, length);
 453                 rc = TPM2_RC_SIZE;
 454                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 455                 return 0;
 456         }
 457
 458         command = get_unaligned_be32(sent);
 459         sent += sizeof(command);
 460         rc = sandbox_tpm2_check_readyness(dev, command);
 461         if (rc) {
 462                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 463                 return 0;
 464         }
 465
 466         rc = sandbox_tpm2_check_session(dev, command, tag, &sent, &hierarchy);
 467         if (rc) {
 468                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 469                 return 0;
 470         }
 471
 472         switch (command) {
 473         case TPM2_CC_STARTUP:
 474                 mode = get_unaligned_be16(sent);
 475                 sent += sizeof(mode);
 476                 switch (mode) {
 477                 case TPM2_SU_CLEAR:
 478                 case TPM2_SU_STATE:
 479                         break;
 480                 default:
 481                         rc = TPM2_RC_VALUE;
 482                 }
 483
 484                 tpm->startup_done = true;
 485
 486                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 487                 break;
 488
 489         case TPM2_CC_SELF_TEST:
 490                 yes_no = *sent;
 491                 sent += sizeof(yes_no);
 492                 switch (yes_no) {
 493                 case TPMI_YES:
 494                 case TPMI_NO:
 495                         break;
 496                 default:
 497                         rc = TPM2_RC_VALUE;
 498                 }
 499
 500                 tpm->tests_done = true;
 501
 502                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 503                 break;
 504
 505         case TPM2_CC_CLEAR:
 506                 /* Reset this hierarchy password */
 507                 tpm->pw_sz[hierarchy] = 0;
 508
 509                 /* Reset all password if thisis the PLATFORM hierarchy */
 510                 if (hierarchy == TPM2_HIERARCHY_PLATFORM)
 511                         for (i = 0; i < TPM2_HIERARCHY_NB; i++)
 512                                 tpm->pw_sz[i] = 0;
 513
 514                 /* Reset the properties */
 515                 for (i = 0; i < TPM2_PROPERTY_NB; i++)
 516                         tpm->properties[i] = 0;
 517
 518                 /* Reset the PCRs and their number of extensions */
 519                 for (i = 0; i < SANDBOX_TPM_PCR_NB; i++) {
 520                         tpm->pcr_extensions[i] = 0;
 521                         for (j = 0; j < TPM2_DIGEST_LEN; j++)
 522                                 tpm->pcr[i][j] = 0;
 523                 }
 524
 525                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 526                 break;
 527
 528         case TPM2_CC_HIERCHANGEAUTH:
 529                 new_pw_sz = get_unaligned_be16(sent);
 530                 sent += sizeof(new_pw_sz);
 531                 if (new_pw_sz > TPM2_DIGEST_LEN) {
 532                         rc = TPM2_RC_SIZE;
 533                 } else if (new_pw_sz) {
 534                         tpm->pw_sz[hierarchy] = new_pw_sz;
 535                         memcpy(tpm->pw[hierarchy], sent, new_pw_sz);
 536                         sent += new_pw_sz;
 537                 }
 538
 539                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 540                 break;
 541
 542         case TPM2_CC_GET_CAPABILITY:
 543                 capability = get_unaligned_be32(sent);
 544                 sent += sizeof(capability);
 545                 if (capability != TPM_CAP_TPM_PROPERTIES) {
 546                         printf("Sandbox TPM only support TPM_CAPABILITIES\n");
 547                         return TPM2_RC_HANDLE;
 548                 }
 549
 550                 property = get_unaligned_be32(sent);
 551                 sent += sizeof(property);
 552                 property -= TPM2_PROPERTIES_OFFSET;
 553
 554                 property_count = get_unaligned_be32(sent);
 555                 sent += sizeof(property_count);
 556                 if (!property_count ||
 557                     property + property_count > TPM2_PROPERTY_NB) {
 558                         rc = TPM2_RC_HANDLE;
 559                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 560                 }
 561
 562                 /* Write tag */
 563                 put_unaligned_be16(tag, recv);
 564                 recv += sizeof(tag);
 565
 566                 /* Ignore length for now */
 567                 recv += sizeof(u32);
 568
 569                 /* Write return code */
 570                 put_unaligned_be32(rc, recv);
 571                 recv += sizeof(rc);
 572
 573                 /* Tell there is more data to read */
 574                 *recv = TPMI_YES;
 575                 recv += sizeof(yes_no);
 576
 577                 /* Repeat the capability */
 578                 put_unaligned_be32(capability, recv);
 579                 recv += sizeof(capability);
 580
 581                 /* Give the number of properties that follow */
 582                 put_unaligned_be32(property_count, recv);
 583                 recv += sizeof(property_count);
 584
 585                 /* Fill with the properties */
 586                 for (i = 0; i < property_count; i++) {
 587                         put_unaligned_be32(TPM2_PROPERTIES_OFFSET + property +
 588                                            i, recv);
 589                         recv += sizeof(property);
 590                         put_unaligned_be32(tpm->properties[property + i],
 591                                            recv);
 592                         recv += sizeof(property);
 593                 }
 594
 595                 /* Add trailing \0 */
 596                 *recv = '\0';
 597
 598                 /* Write response length */
 599                 *recv_len = recv - recvbuf;
 600                 put_unaligned_be32(*recv_len, recvbuf + sizeof(tag));
 601
 602                 break;
 603
 604         case TPM2_CC_DAM_PARAMETERS:
 605                 tpm->properties[TPM2_PROP_MAX_TRIES] = get_unaligned_be32(sent);
 606                 sent += sizeof(*tpm->properties);
 607                 tpm->properties[TPM2_RECOVERY_TIME] = get_unaligned_be32(sent);
 608                 sent += sizeof(*tpm->properties);
 609                 tpm->properties[TPM2_LOCKOUT_RECOVERY] = get_unaligned_be32(sent);
 610                 sent += sizeof(*tpm->properties);
 611
 612                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 613                 break;
 614
 615         case TPM2_CC_PCR_READ:
 616                 selections = get_unaligned_be32(sent);
 617                 sent += sizeof(selections);
 618                 if (selections != 1) {
 619                         printf("Sandbox cannot handle more than one PCR\n");
 620                         rc = TPM2_RC_VALUE;
 621                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 622                 }
 623
 624                 alg = get_unaligned_be16(sent);
 625                 sent += sizeof(alg);
 626                 if (alg != TPM2_ALG_SHA256) {
 627                         printf("Sandbox TPM only handle SHA256 algorithm\n");
 628                         rc = TPM2_RC_VALUE;
 629                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 630                 }
 631
 632                 pcr_array_sz = *sent;
 633                 sent += sizeof(pcr_array_sz);
 634                 if (!pcr_array_sz || pcr_array_sz > 8) {
 635                         printf("Sandbox TPM cannot handle so much PCRs\n");
 636                         rc = TPM2_RC_VALUE;
 637                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 638                 }
 639
 640                 for (i = 0; i < pcr_array_sz; i++)
 641                         pcr_map += (u64)sent[i] << (i * 8);
 642
 643                 if (!pcr_map) {
 644                         printf("Empty PCR map\n");
 645                         rc = TPM2_RC_VALUE;
 646                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 647                 }
 648
 649                 for (i = 0; i < SANDBOX_TPM_PCR_NB; i++)
 650                         if (pcr_map & BIT(i))
 651                                 pcr_index = i;
 652
 653                 if (pcr_index >= SANDBOX_TPM_PCR_NB) {
 654                         printf("Invalid index %d, sandbox TPM handles up to %d PCR(s)\n",
 655                                pcr_index, SANDBOX_TPM_PCR_NB);
 656                         rc = TPM2_RC_VALUE;
 657                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 658                 }
 659
 660                 /* Write tag */
 661                 put_unaligned_be16(tag, recv);
 662                 recv += sizeof(tag);
 663
 664                 /* Ignore length for now */
 665                 recv += sizeof(u32);
 666
 667                 /* Write return code */
 668                 put_unaligned_be32(rc, recv);
 669                 recv += sizeof(rc);
 670
 671                 /* Number of extensions */
 672                 put_unaligned_be32(tpm->pcr_extensions[pcr_index], recv);
 673                 recv += sizeof(u32);
 674
 675                 /* Copy the PCR */
 676                 memcpy(recv, tpm->pcr[pcr_index], TPM2_DIGEST_LEN);
 677                 recv += TPM2_DIGEST_LEN;
 678
 679                 /* Add trailing \0 */
 680                 *recv = '\0';
 681
 682                 /* Write response length */
 683                 *recv_len = recv - recvbuf;
 684                 put_unaligned_be32(*recv_len, recvbuf + sizeof(tag));
 685
 686                 break;
 687
 688         case TPM2_CC_PCR_EXTEND:
 689                 /* Get the PCR index */
 690                 pcr_index = get_unaligned_be32(sendbuf + sizeof(tag) +
 691                                                sizeof(length) +
 692                                                sizeof(command));
 693                 if (pcr_index >= SANDBOX_TPM_PCR_NB) {
 694                         printf("Invalid index %d, sandbox TPM handles up to %d PCR(s)\n",
 695                                pcr_index, SANDBOX_TPM_PCR_NB);
 696                         rc = TPM2_RC_VALUE;
 697                 }
 698
 699                 /* Check the number of hashes */
 700                 pcr_nb = get_unaligned_be32(sent);
 701                 sent += sizeof(pcr_nb);
 702                 if (pcr_nb != 1) {
 703                         printf("Sandbox cannot handle more than one PCR\n");
 704                         rc = TPM2_RC_VALUE;
 705                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 706                 }
 707
 708                 /* Check the hash algorithm */
 709                 alg = get_unaligned_be16(sent);
 710                 sent += sizeof(alg);
 711                 if (alg != TPM2_ALG_SHA256) {
 712                         printf("Sandbox TPM only handle SHA256 algorithm\n");
 713                         rc = TPM2_RC_VALUE;
 714                         return sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 715                 }
 716
 717                 /* Extend the PCR */
 718                 rc = sandbox_tpm2_extend(dev, pcr_index, sent);
 719
 720                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 721                 break;
 722
 723         case TPM2_CC_NV_READ: {
 724                 int index, seq;
 725
 726                 index = get_unaligned_be32(sendbuf + TPM2_HDR_LEN + 4);
 727                 length = get_unaligned_be16(sent);
 728                 /* ignore offset */
 729                 seq = sb_tpm_index_to_seq(index);
 730                 if (seq < 0)
 731                         return log_msg_ret("index", -EINVAL);
 732                 printf("tpm: nvread index=%#02x, len=%#02x, seq=%#02x\n", index,
 733                        length, seq);
 734                 *recv_len = TPM2_HDR_LEN + 6 + length;
 735                 memset(recvbuf, '\0', *recv_len);
 736                 put_unaligned_be32(length, recvbuf + 2);
 737                 sb_tpm_read_data(tpm->nvdata, seq, recvbuf,
 738                                  TPM2_HDR_LEN + 4 + 2, length);
 739                 break;
 740         }
 741         case TPM2_CC_NV_WRITE: {
 742                 int index, seq;
 743
 744                 index = get_unaligned_be32(sendbuf + TPM2_HDR_LEN + 4);
 745                 length = get_unaligned_be16(sent);
 746                 sent += sizeof(u16);
 747
 748                 /* ignore offset */
 749                 seq = sb_tpm_index_to_seq(index);
 750                 if (seq < 0)
 751                         return log_msg_ret("index", -EINVAL);
 752                 printf("tpm: nvwrite index=%#02x, len=%#02x, seq=%#02x\n", index,
 753                        length, seq);
 754                 memcpy(&tpm->nvdata[seq].data, sent, length);
 755                 tpm->nvdata[seq].present = true;
 756                 *recv_len = TPM2_HDR_LEN + 2;
 757                 memset(recvbuf, '\0', *recv_len);
 758                 break;
 759         }
 760         case TPM2_CC_NV_DEFINE_SPACE: {
 761                 int policy_size, index, seq;
 762
 763                 policy_size = get_unaligned_be16(sent + 12);
 764                 index = get_unaligned_be32(sent + 2);
 765                 sent += 14 + policy_size;
 766                 length = get_unaligned_be16(sent);
 767                 seq = sb_tpm_index_to_seq(index);
 768                 if (seq < 0)
 769                         return -EINVAL;
 770                 printf("tpm: define_space index=%x, len=%x, seq=%x, policy_size=%x\n",
 771                        index, length, seq, policy_size);
 772                 sb_tpm_define_data(tpm->nvdata, seq, length);
 773                 *recv_len = 12;
 774                 memset(recvbuf, '\0', *recv_len);
 775                 break;
 776         }
 777         case TPM2_CC_NV_WRITELOCK:
 778                 *recv_len = 12;
 779                 memset(recvbuf, '\0', *recv_len);
 780                 break;
 781         default:
 782                 printf("TPM2 command %02x unknown in Sandbox\n", command);
 783                 rc = TPM2_RC_COMMAND_CODE;
 784                 sandbox_tpm2_fill_buf(recv, recv_len, tag, rc);
 785         }
 786
 787         return 0;
 788 }
 789
 790 static int sandbox_tpm2_get_desc(struct udevice *dev, char *buf, int size)
 791 {
 792         if (size < 15)
 793                 return -ENOSPC;
 794
 795         return snprintf(buf, size, "Sandbox TPM2.x");
 796 }
 797
 798 static int sandbox_tpm2_report_state(struct udevice *dev, char *buf, int size)
 799 {
 800         struct sandbox_tpm2 *priv = dev_get_priv(dev);
 801
 802         if (size < 40)
 803                 return -ENOSPC;
 804
 805         return snprintf(buf, size, "init_done=%d", priv->init_done);
 806 }
 807
 808 static int sandbox_tpm2_open(struct udevice *dev)
 809 {
 810         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 811
 812         if (tpm->init_done)
 813                 return -EBUSY;
 814
 815         tpm->init_done = true;
 816
 817         return 0;
 818 }
 819
 820 static int sandbox_tpm2_probe(struct udevice *dev)
 821 {
 822         struct sandbox_tpm2 *tpm = dev_get_priv(dev);
 823         struct tpm_chip_priv *priv = dev_get_uclass_priv(dev);
 824
 825         /* Use the TPM v2 stack */
 826         priv->version = TPM_V2;
 827
 828         priv->pcr_count = 32;
 829         priv->pcr_select_min = 2;
 830
 831         if (s_state.valid)
 832                 memcpy(tpm, &s_state, sizeof(*tpm));
 833         g_state = tpm;
 834
 835         return 0;
 836 }
 837
 838 static int sandbox_tpm2_close(struct udevice *dev)
 839 {
 840         return 0;
 841 }
 842
 843 static const struct tpm_ops sandbox_tpm2_ops = {
 844         .open           = sandbox_tpm2_open,
 845         .close          = sandbox_tpm2_close,
 846         .get_desc       = sandbox_tpm2_get_desc,
 847         .report_state   = sandbox_tpm2_report_state,
 848         .xfer           = sandbox_tpm2_xfer,
 849 };
 850
 851 static const struct udevice_id sandbox_tpm2_ids[] = {
 852         { .compatible = "sandbox,tpm2" },
 853         { }
 854 };
 855
 856 U_BOOT_DRIVER(sandbox_tpm2) = {
 857         .name   = "sandbox_tpm2",
 858         .id     = UCLASS_TPM,
 859         .of_match = sandbox_tpm2_ids,
 860         .ops    = &sandbox_tpm2_ops,
 861         .probe  = sandbox_tpm2_probe,
 862         .priv_auto      = sizeof(struct sandbox_tpm2),
 863 };