Diff: SPITIS_TPM20.cpp

Revision:

0:b11c8971edd9

Child:

1:fd0a59e55a85

diff -r 000000000000 -r b11c8971edd9 SPITIS_TPM20.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SPITIS_TPM20.cpp	Mon Mar 23 19:03:50 2015 +0000
@@ -0,0 +1,874 @@
+/* mbed TCG TIS 1.3 SPI TPM 2.0 Library,
+ * Copyright (c) 2015, Microsoft Coprporation Inc.
+ * by Stefan Thom (LordOfDorks) StefanTh@Microsoft.com, Stefan@ThomsR.Us
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+ 
+ #include "SPITIS_TPM20.h"
+ 
+TISTPM20::TISTPM20(
+    PinName mosi,
+    PinName miso,
+    PinName clk,
+    PinName cs
+    )
+{
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Init: ");
+#endif
+    m_SPITpmDev = new SPI(mosi, miso, clk);
+    m_SPITpmDev->format(8, 0);
+    m_SPITpmDev->frequency(4000000);
+    m_SPICSTpmDev = new DigitalOut(cs);
+    *m_SPICSTpmDev = 1;
+    m_ExclusiveAccess = false;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("OK.\n\r");
+#endif
+}
+
+// Release all held resources
+TISTPM20::~TISTPM20()
+{
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Destroy: ");
+#endif
+    if(m_SPITpmDev != NULL)
+    {
+        delete m_SPITpmDev;
+        m_SPITpmDev = NULL;
+    }
+    if(m_SPICSTpmDev != NULL)
+    {
+        delete m_SPICSTpmDev;
+        m_SPICSTpmDev = NULL;
+    }
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("OK.\n\r");
+#endif
+}
+
+uint32_t
+TISTPM20::Execute(
+    uint8_t locality,
+    uint8_t* pbCmd,
+    uint32_t cbCmd,
+    uint8_t* pbRsp,
+    uint32_t cbRsp,
+    uint32_t timeout
+    )
+{
+    uint32_t result = 0;
+    Timeout watchdog;
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.ExecuteWaitForAccess.");
+#endif
+
+    // Only one caller should be talking to the TPM at any given time
+    while(m_ExclusiveAccess)
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf(".");
+#endif
+        wait_us(500);
+    }
+    m_ExclusiveAccess = true;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("OK\n\r");
+#endif
+
+    // Set the requested locality for the call
+    m_Locality = locality;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Locality = %d\n\r", m_Locality);
+#endif
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.SetupTimeout\n\r");
+#endif
+    // Setup TPM timeout
+    m_TimeoutTriggered = false;
+    watchdog.attach(this, &TISTPM20::TimeoutTrigger, 0.0001 * timeout);
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Execute: ");
+    for(uint32_t n = 0; n < cbCmd; n++) printf("%02x ", pbCmd[n]);
+    printf("\n\r");
+#endif
+    
+    // Execute command on the TPM
+    if((result = ExecuteTIS(pbCmd, cbCmd, pbRsp, cbRsp)) == 0)
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.Failed\n\r");
+#endif
+        goto Cleanup;
+    }
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Response: ");
+    for(uint32_t n = 0; n < result; n++) printf("%02x ", pbRsp[n]);
+    printf("\n\r");
+#endif
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.CancelTimeout\n\r");
+#endif
+
+Cleanup:
+    watchdog.detach();
+    m_ExclusiveAccess = false;
+    return result;
+}
+
+uint32_t
+TISTPM20::ParseResponseHeader(
+    uint8_t* pbRsp,
+    uint32_t rspLen,
+    uint16_t* rspTag,
+    uint32_t* rspSize,
+    uint32_t* cursor
+    )
+{
+    uint32_t rspResponseCode = 0;
+
+    // Check that the response header is well formatted
+    if(rspLen < (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t)))
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.ResponseHdr.rspLen = 0x%08x\n\r", rspLen);
+#endif
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+    
+    // Read the header components
+    *rspTag = BYTEARRAY_TO_UINT16(pbRsp, *cursor);
+    *cursor += sizeof(uint16_t);
+    *rspSize = BYTEARRAY_TO_UINT32(pbRsp, *cursor);
+    *cursor += sizeof(uint32_t);
+    rspResponseCode = BYTEARRAY_TO_UINT32(pbRsp, *cursor);
+    *cursor += sizeof(uint32_t);
+
+    // Check the components
+    if(((*rspTag != TPM_ST_NO_SESSIONS) && (*rspTag != TPM_ST_SESSIONS)) ||
+       (*rspSize != rspLen))
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.ResponseHdr.rspTag = 0x%04x.rspLen=0x%08x\n\r", *rspTag, rspLen);
+#endif
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+
+Cleanup:    
+    return rspResponseCode;
+}
+
+uint32_t
+TISTPM20::TPM2_Startup(
+    uint8_t locality, 
+    uint16_t startupType
+    )
+{
+    uint32_t rspLen = 0;
+    uint16_t rspTag = 0;
+    uint32_t rspSize = 0;
+    uint32_t rspResponseCode = 0;
+    uint32_t cursor = 0;
+    uint8_t tpmCmd[] = {0x80, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x01, 0x44, 0x00, 0x00};
+#ifndef TPM_TIS_DEBUG_OUTPUT
+    uint32_t timeout = 2000;
+#else
+    uint32_t timeout = 20000;
+#endif
+    UINT16_TO_BYTEARRAY(startupType, tpmCmd, sizeof(tpmCmd) - 2);  
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_Startup(0x%04x)\n\r", startupType);
+#endif
+
+    if((rspLen = Execute(locality, tpmCmd, sizeof(tpmCmd), tpmCmd, sizeof(tpmCmd), timeout)) == 0)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+    if((rspResponseCode = ParseResponseHeader(tpmCmd, rspLen, &rspTag, &rspSize, &cursor)) != TPM_RC_SUCCESS)
+    {
+        goto Cleanup;
+    }
+    
+    if(rspSize != 0x0000000a)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+    }
+
+Cleanup:    
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_Startup.ResponseCode = 0x%08x\n\r", rspResponseCode);
+#endif
+    return rspResponseCode;
+}
+
+uint32_t
+TISTPM20::TPM2_Shutdown(
+    uint8_t locality, 
+    uint16_t shutdownType
+    )
+{
+    uint32_t rspLen = 0;
+    uint16_t rspTag = 0;
+    uint32_t rspSize = 0;
+    uint32_t rspResponseCode = 0;
+    uint32_t cursor = 0;
+    uint8_t tpmCmd[] = {0x80, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x01, 0x45, 0x00, 0x00};
+#ifndef TPM_TIS_DEBUG_OUTPUT
+    uint32_t timeout = 2000;
+#else
+    uint32_t timeout = 20000;
+#endif
+    UINT16_TO_BYTEARRAY(shutdownType, tpmCmd, sizeof(tpmCmd) - 2);
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_Shutdown(0x%04x)\n\r", shutdownType);
+#endif
+
+    if((rspLen = Execute(locality, tpmCmd, sizeof(tpmCmd), tpmCmd, sizeof(tpmCmd), timeout)) == 0)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+    if((rspResponseCode = ParseResponseHeader(tpmCmd, rspLen, &rspTag, &rspSize, &cursor)) != TPM_RC_SUCCESS)
+    {
+        goto Cleanup;
+    }
+
+    if(rspSize != 0x0000000a)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+    }
+
+Cleanup:
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_Shutdown.ResponseCode = 0x%08x\n\r", rspResponseCode);
+#endif
+    return rspResponseCode;
+}
+
+uint32_t
+TISTPM20::TPM2_SelfTest(
+    uint8_t locality, 
+    uint8_t fullTest
+    )
+{
+    uint32_t rspLen = 0;
+    uint16_t rspTag = 0;
+    uint32_t rspSize = 0;
+    uint32_t rspResponseCode = 0;
+    uint32_t cursor = 0;
+    uint8_t tpmCmd[] = {0x80, 0x01, 0x00, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x01, 0x43, 0x00};
+#ifndef TPM_TIS_DEBUG_OUTPUT
+    uint32_t timeout = 2000;
+#else
+    uint32_t timeout = 20000;
+#endif
+    tpmCmd[sizeof(tpmCmd) - 1] = fullTest;
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_SelfTest(0x%02x)\n\r", fullTest);
+#endif
+
+    if((rspLen = Execute(locality, tpmCmd, sizeof(tpmCmd), tpmCmd, sizeof(tpmCmd), timeout)) == 0)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+    if((rspResponseCode = ParseResponseHeader(tpmCmd, rspLen, &rspTag, &rspSize, &cursor)) != TPM_RC_SUCCESS)
+    {
+        goto Cleanup;
+    }
+
+    if(rspSize != 0x0000000a)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+    }
+
+Cleanup:
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_SelfTest.ResponseCode = 0x%08x\n\r", rspResponseCode);
+#endif
+    return rspResponseCode;
+}
+
+uint32_t
+TISTPM20::TPM2_GetRandom(
+    uint8_t locality, 
+    uint16_t bytesRequested,
+    uint8_t* randomBytes
+    )
+{
+    uint32_t cursor = 0;
+    uint32_t rspLen = 0;
+    uint16_t rspTag = 0;
+    uint32_t rspSize = 0;
+    uint32_t rspResponseCode = 0;
+    uint32_t tpmMax = sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + bytesRequested;
+    uint8_t* tpmCmd = new uint8_t[tpmMax];
+    uint16_t bytesReturned = 0;
+#ifndef TPM_TIS_DEBUG_OUTPUT
+    uint32_t timeout = 2000;
+#else
+    uint32_t timeout = 20000;
+#endif
+    
+    if(tpmCmd == NULL)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+
+    // Build command
+    UINT16_TO_BYTEARRAY(TPM_ST_NO_SESSIONS, tpmCmd, cursor);
+    cursor += sizeof(uint16_t) + sizeof(cursor);
+    UINT32_TO_BYTEARRAY(TPM_CC_GetRandom, tpmCmd, cursor);
+    cursor += sizeof(TPM_CC_GetRandom);
+    UINT16_TO_BYTEARRAY(bytesRequested, tpmCmd, cursor);
+    cursor += sizeof(bytesRequested);
+    UINT32_TO_BYTEARRAY(cursor, tpmCmd, sizeof(uint16_t));
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_GetRandom(%d)\n\r", bytesRequested);
+#endif
+
+    if((rspLen = Execute(locality, tpmCmd, cursor, tpmCmd, tpmMax, timeout)) == 0)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+    cursor = 0;
+    if((rspResponseCode = ParseResponseHeader(tpmCmd, rspLen, &rspTag, &rspSize, &cursor)) != TPM_RC_SUCCESS)
+    {
+        goto Cleanup;
+    }
+
+    if(rspSize != tpmMax)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+
+    // Copy the random bytes out
+    bytesReturned = BYTEARRAY_TO_UINT16(tpmCmd, cursor);
+    cursor += sizeof(uint16_t);
+    memcpy(randomBytes, &tpmCmd[cursor], (size_t)min(bytesReturned, bytesRequested));
+ 
+ Cleanup:
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_GetRandom.ResponseCode = 0x%08x\n\r", rspResponseCode);
+#endif
+    if(tpmCmd != NULL)
+    {
+        delete[] tpmCmd;
+        tpmCmd = NULL;
+    }
+    return rspResponseCode;
+}
+
+uint32_t
+TISTPM20::TPM2_StirRandom(
+    uint8_t locality, 
+    uint16_t inDataLen,
+    uint8_t* inData
+    )
+{
+    uint32_t cursor = 0;
+    uint32_t rspLen = 0;
+    uint16_t rspTag = 0;
+    uint32_t rspSize = 0;
+    uint32_t rspResponseCode = 0;
+    uint32_t tpmMax = sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + inDataLen;
+    uint8_t* tpmCmd = new uint8_t[tpmMax];
+ #ifndef TPM_TIS_DEBUG_OUTPUT
+    uint32_t timeout = 2000;
+#else
+    uint32_t timeout = 20000;
+#endif
+   
+    if(tpmCmd == NULL)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+
+    // Build command
+    UINT16_TO_BYTEARRAY(TPM_ST_NO_SESSIONS, tpmCmd, cursor);
+    cursor += sizeof(uint16_t) + sizeof(cursor);
+    UINT32_TO_BYTEARRAY(TPM_CC_StirRandom, tpmCmd, cursor);
+    cursor += sizeof(TPM_CC_GetRandom);
+    UINT16_TO_BYTEARRAY(inDataLen, tpmCmd, cursor);
+    cursor += sizeof(inDataLen);
+    memcpy(&tpmCmd[cursor], inData, inDataLen);
+    cursor += inDataLen;
+    UINT32_TO_BYTEARRAY(cursor, tpmCmd, sizeof(uint16_t));
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_StirRandom(%d)\n\r", inDataLen);
+#endif
+
+    if((rspLen = Execute(locality, tpmCmd, cursor, tpmCmd, tpmMax, timeout)) == 0)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+    cursor = 0;
+    if((rspResponseCode = ParseResponseHeader(tpmCmd, rspLen, &rspTag, &rspSize, &cursor)) != TPM_RC_SUCCESS)
+    {
+        goto Cleanup;
+    }
+
+    if(rspSize != 0x0000000a)
+    {
+        rspResponseCode = TPM_RC_FAILURE;
+        goto Cleanup;
+    }
+
+ Cleanup:
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.TPM2_StirRandom.ResponseCode = 0x%08x\n\r", rspResponseCode);
+#endif
+    if(tpmCmd != NULL)
+    {
+        delete[] tpmCmd;
+        tpmCmd = NULL;
+    }
+    return rspResponseCode;
+}
+
+void
+TISTPM20::TimeoutTrigger(
+    void
+    )
+{
+    m_TimeoutTriggered = true;
+}
+
+uint32_t
+TISTPM20::ExecuteTIS(
+    uint8_t* pbCmd,
+    uint32_t cbCmd,
+    uint8_t* pbRsp,
+    uint32_t cbRsp
+    )
+{
+    uint32_t result = 0;
+    uint8_t tisStatus = 0xFF;
+    uint32_t index = 0;
+    uint32_t rspSize = 0;
+    uint16_t tisBurstCount = 0;
+
+    // Lock the requested locality
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.RequestLocality\r\n");
+#endif
+    if(!RequestLocality())
+    {
+        goto Cleanup;
+    }
+    
+    // Check the status before writing the command
+    if(!GetStatus(&tisStatus))
+    {
+        goto Cleanup;
+    }
+    // Abort anything that may still be stuck in the TPM
+    if((tisStatus & TIS_STS_COMMAND_READY) == 0)
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.GetReady\r\n");
+#endif
+        uint8_t mask = TIS_STS_COMMAND_READY;
+        if(!Abort() || !WaitForStatus(mask))
+        {
+            goto Cleanup;
+        }
+    }
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.ReadyForCommand\r\n");
+#endif
+
+    // Send the command to the TPM
+    do
+    {
+        uint16_t cbIteration = 0;
+
+        // Get the maximum bytes we can send for this iteration
+        if((!GetBurstCount(&tisBurstCount)) || (tisBurstCount == 0))
+        {
+            goto Cleanup;
+        }
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.GetBurstCount = %d\r\n", tisBurstCount);
+#endif
+
+        // Assemble the buffer for transmission
+        cbIteration = min((cbCmd - index), min(tisBurstCount, TIS_MAX_HW_FRAME_SIZE));
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.SendingBurst = %d\r\n", cbIteration);
+#endif
+        if(!SPIFullDuplex(false, TIS_DATA_FIFO, &pbCmd[index], cbIteration))
+        {
+            goto Cleanup;
+        }
+
+        // Update the counts
+        index += cbIteration;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.Remaining = %d\r\n", (cbCmd - index));
+#endif
+    } while((cbCmd - index) > 0);
+
+    // Wait for the TPM to get ready and kick the command off
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.StatusValid\r\n");
+#endif
+    if(!WaitForStatus(TIS_STS_VALID) || !Go())
+    {
+        goto Cleanup;
+    }
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Go\r\n");
+#endif
+
+    // Wait for the TPM to finish the command execution
+    do
+    {
+        wait_us(1000);
+        tisStatus = 0xFF;
+        if(!GetStatus(&tisStatus) || ((tisStatus & TIS_STS_VALID) == 0))
+        {
+            goto Cleanup;
+        }
+    }
+    while(((tisStatus & TIS_STS_DATA_AVAIL) == 0) && (!m_TimeoutTriggered));
+
+    if(m_TimeoutTriggered)
+    {
+        goto Cleanup;
+    }
+
+    // Get the response header from the TPM
+    index = 0;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("Tis.ReadResponseHeader\r\n");
+#endif
+    if(!GetBurstCount(&tisBurstCount) ||
+       (tisBurstCount < (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t))) ||
+       !SPIFullDuplex(true, TIS_DATA_FIFO, &pbRsp[index], (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t))))
+    {
+        goto Cleanup;
+    }
+    index += (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t));
+   
+    rspSize = BYTEARRAY_TO_UINT32(pbRsp, sizeof(uint16_t));
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("Tis.ResponseSize = %d\r\n", rspSize);
+#endif
+    
+    if(rspSize > index)
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("Tis.ReadResponse\r\n");
+#endif
+        do
+        {
+            uint16_t cbIteration = 0;
+
+            // Check to make sure the TPM has still data for us
+            if(!GetStatus(&tisStatus) || ((tisStatus & TIS_STS_DATA_AVAIL) == 0))
+            {
+                goto Cleanup;
+            }
+
+            // Get the number of available bytes for reading in this iteration
+            if(!GetBurstCount(&tisBurstCount))
+            {
+                goto Cleanup;
+            }
+#ifdef TPM_TIS_DEBUG_OUTPUT
+            printf("TIS.GetBurstCount = %d\r\n", tisBurstCount);
+#endif
+
+            cbIteration = min((rspSize - index), min((cbRsp - index), min(tisBurstCount, TIS_MAX_HW_FRAME_SIZE)));
+#ifdef TPM_TIS_DEBUG_OUTPUT
+            printf("TIS.ReceivingBurst = %d\r\n", cbIteration);
+#endif
+
+            // Read the data for this iteration
+            if(!SPIFullDuplex(true, TIS_DATA_FIFO, &pbRsp[index], cbIteration))
+            {
+                goto Cleanup;
+            }
+            index += cbIteration;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+            printf("TIS.Remaining = %d\r\n", (min(rspSize, cbRsp) - index));
+#endif
+        }
+        while(index < min(rspSize, cbRsp));
+    }
+    
+    result = index;
+    
+Cleanup:
+    // Always leave the TPM in a clean state if somethign fishy happened
+    if(result == 0)
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.Abort\r\n");
+#endif
+        Abort();
+    }
+
+    // Release the locality again
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.ReleaseLocality\r\n");
+#endif
+    ReleaseLocality();
+    return result;
+}
+
+bool
+TISTPM20::SPIFullDuplex(
+    bool readCycle,
+    uint8_t tisRegister,
+    uint8_t* pbBuffer,
+    uint16_t cbBuffer
+    )
+{
+    bool result = false;
+    uint8_t dataByteIn;
+    uint8_t dataByteOut;
+
+    // Lock the bus for this operation
+    *m_SPICSTpmDev = 0;
+    
+    // Send the TIS header
+    uint32_t tisHdr = TIS_HEADER(m_Locality, readCycle, tisRegister, cbBuffer);
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS(LC:%d,RG:%02x,SZ:%02x,%s):", m_Locality, tisRegister, cbBuffer, (readCycle) ? "RD" : "WR");
+#endif
+
+    for(uint8_t n = 0; n < sizeof(tisHdr); n++)
+    {
+         dataByteOut = tisHdr >> (8 * (3 - n));
+         dataByteIn = m_SPITpmDev->write(dataByteOut);
+    }
+        
+    // The last read bit is the wait state indicator
+    if((dataByteIn & 0x01) == 0)
+    {
+        do
+        {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+            printf(".");
+#endif
+            // Do we still have to wait?
+            if((dataByteIn = m_SPITpmDev->write(0xFF)) != 0x01)
+            {
+                wait_us(10);
+            }
+        }
+        while((dataByteIn != 0x01) && (!m_TimeoutTriggered));
+        
+        // Check if we timed out
+        if((dataByteIn != 0x01) && (m_TimeoutTriggered))
+        {
+            goto Cleanup;
+        }
+    }
+    
+    // Do the remaining bytes now
+    for(uint8_t n = 0; n < cbBuffer; n++)
+    {
+        dataByteOut = (readCycle) ? 0xFF : pbBuffer[n];
+        dataByteIn = m_SPITpmDev->write(dataByteOut);
+        if(readCycle) pbBuffer[n] = dataByteIn;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("0x%02x ", (readCycle) ? dataByteIn : dataByteOut);
+#endif
+    }
+    result = true;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("\r\n");
+#endif
+
+Cleanup:
+    // Make sure to release the bus before we leave
+    *m_SPICSTpmDev = 1;
+    return result;
+}
+
+bool
+TISTPM20::RequestLocality()
+{
+    do
+    {
+        // Do we have access already?
+        uint8_t dataByte = 0xFF;
+        if(!SPIFullDuplex(true, TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+        {
+            return false;
+        }
+        if((dataByte & TIS_ACCESS_ACTIVE_LOCALITY) &&
+           (dataByte & TIS_ACCESS_VALID))
+        {
+            break;
+        }
+
+        // Request access
+        dataByte = TIS_ACCESS_REQUEST_USE;
+        if(!SPIFullDuplex(false, TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+        {
+            return false;
+        }
+        if(m_TimeoutTriggered)
+        {
+            // We give up eventually
+            return false;
+        }
+        wait_us(10);
+    }
+    while(1);
+    return true;
+}
+
+bool TISTPM20::ReleaseLocality()
+{
+    do
+    {
+        // Do we even have access?
+        uint8_t dataByte = 0xFF;
+        if(!SPIFullDuplex(true, TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+        {
+            return false;
+        }
+        if(!(dataByte & TIS_ACCESS_ACTIVE_LOCALITY) &&
+            (dataByte & TIS_ACCESS_VALID))
+        {
+            break;
+        }
+
+        // Release access
+        dataByte = TIS_ACCESS_ACTIVE_LOCALITY;
+        if(!SPIFullDuplex(false, TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+        {
+            return false;
+        }
+        if(m_TimeoutTriggered)
+        {
+            // We give up eventually
+            return false;
+        }
+        wait_us(10);
+    }
+    while(1);
+    return true;
+}
+
+bool
+TISTPM20::GetBurstCount(
+    uint16_t* pBurstCount
+    )
+{
+    uint8_t dataBytes[sizeof(*pBurstCount)] = {0};
+    do
+    {
+        if(!SPIFullDuplex(true, TIS_BURSTCOUNT_REGISTER, dataBytes, sizeof(dataBytes)))
+        {
+            return false;
+        }
+        *pBurstCount = (uint16_t)dataBytes[0] | (uint16_t)(dataBytes[1] << 8);
+
+        // Burst count can be 0 if we are too fast
+        if(*pBurstCount != 0)
+        {
+            break;
+        }
+        if(m_TimeoutTriggered)
+        {
+            // We give up eventually
+            return false;
+        }
+        wait_us(10);
+     }
+    while(1);
+    return true;  
+}
+
+bool
+TISTPM20::GetStatus(
+    uint8_t* pStatus
+    )
+{
+    bool result = SPIFullDuplex(true, TIS_STATUS_REGISTER, pStatus, sizeof(*pStatus));
+    return result;
+}
+
+bool
+TISTPM20::WaitForStatus(
+    uint8_t statusMask
+    )
+{
+    bool result = false;
+    uint8_t tisStatus = 0xFF;
+
+    do
+    {
+        if(!GetStatus(&tisStatus))
+        {
+            goto Cleanup;
+        }
+        if((tisStatus & statusMask) == statusMask)
+        {
+            result = true;
+            break;
+        }
+        wait_us(10);
+    }
+    while(!m_TimeoutTriggered);
+
+Cleanup:    
+    return result;
+}
+
+bool
+TISTPM20::Abort()
+{
+    uint8_t dataByte = TIS_STS_COMMAND_READY;
+    return SPIFullDuplex(false, TIS_STATUS_REGISTER, &dataByte, sizeof(dataByte));
+}
+
+bool
+TISTPM20::Go()
+{
+    uint8_t dataByte = TIS_STS_GO;
+    return SPIFullDuplex(false, TIS_STATUS_REGISTER, &dataByte, sizeof(dataByte));
+}
\ No newline at end of file