Diff: SPITIS_TPM20.cpp

Revision:

1:fd0a59e55a85

Parent:

0:b11c8971edd9

Child:

2:526bf792254d

diff -r b11c8971edd9 -r fd0a59e55a85 SPITIS_TPM20.cpp
--- a/SPITIS_TPM20.cpp	Mon Mar 23 19:03:50 2015 +0000
+++ b/SPITIS_TPM20.cpp	Tue Apr 07 15:57:47 2015 +0000
@@ -1,4 +1,4 @@
-/* mbed TCG TIS 1.3 SPI TPM 2.0 Library,
+/* mbed TCG SPI TPM 2.0 TIS 1.3 driver,
  * Copyright (c) 2015, Microsoft Coprporation Inc.
  * by Stefan Thom (LordOfDorks) StefanTh@Microsoft.com, Stefan@ThomsR.Us
  *
@@ -19,161 +19,412 @@
  * 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
-    )
+
+#include "SPITIS_TPM20.h"
+#include "TPM20Tables.h"
+
+TIS_TPM20::TIS_RESULT
+TIS_TPM20::InitializeTis()
 {
+    TIS_RESULT result = TIS_SESSION_RESULT_COMPLETE;
+    uint8_t dataRegister[sizeof(m_intfCabability)] = {0};
+    
+    if(RequestLocality(TIS_LOCALITY_0) == false)
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+    
+    // Read the TIS capabilities
+    if(ReadRegister(TIS_INTF_CAPABILITY_REGISTER, dataRegister, sizeof(dataRegister)) == false)
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+    m_intfCabability = LE_BYTEARRAY_TO_UINT32(dataRegister, 0);
+
 #ifdef TPM_TIS_DEBUG_OUTPUT
-    printf("TIS.Init: ");
+    printf("TIS.InitializeTis.IntfCapability = 0x%08x\n\r", m_intfCabability);
 #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;
+    
+    // If the TIS interface has a fixed burst count use that number instead of asking the TPM over and over and save cycles
+    if(m_intfCabability & TIS_INTF_CAPPABILITY_BURST_COUNT_STATIC)
+    {     
+        if((m_intfCabability & TIS_INTF_CAPPABILITY_DATA_TRANSFER_SIZE_SUPPORT_MASK) == TIS_INTF_CAPPABILITY_DATA_TRANSFER_SIZE_SUPPORT_8B)
+        {
+            m_fixedBurstCount = 8;
+        }
+        else if((m_intfCabability & TIS_INTF_CAPPABILITY_DATA_TRANSFER_SIZE_SUPPORT_MASK) == TIS_INTF_CAPPABILITY_DATA_TRANSFER_SIZE_SUPPORT_32B)
+        {
+            m_fixedBurstCount = 32;
+        }
+        else if ((m_intfCabability & TIS_INTF_CAPPABILITY_DATA_TRANSFER_SIZE_SUPPORT_MASK) == TIS_INTF_CAPPABILITY_DATA_TRANSFER_SIZE_SUPPORT_64B)
+        {
+            m_fixedBurstCount = 64;
+        }
 #ifdef TPM_TIS_DEBUG_OUTPUT
-    printf("OK.\n\r");
+        printf("TIS.InitializeTis.FixedBurstCount = %d\n\r", m_fixedBurstCount);
 #endif
+    }
+    
+    result = TIS_SESSION_RESULT_COMPLETE;
+
+Cleanup:
+    ReleaseLocality();
+    return result;
 }
 
-// Release all held resources
-TISTPM20::~TISTPM20()
+#ifndef TPM_TPM_TIS_NO_COMMAND_FILTERING
+TIS_TPM20::TIS_RESULT
+TIS_TPM20::ApplyFilter(TIS_TPM20::TIS_LOCALITY locality, uint8_t* pbCmd, uint32_t cbCmd)
 {
-#ifdef TPM_TIS_DEBUG_OUTPUT
-    printf("TIS.Destroy: ");
-#endif
-    if(m_SPITpmDev != NULL)
+    uint32_t cursor = sizeof(uint16_t) + sizeof(uint32_t);
+    uint32_t ordinal = BE_BYTEARRAY_TO_UINT32(pbCmd, cursor);
+    int32_t handleCount = -1;
+
+    // First filter ordinals based on locality
+    if(locality == TIS_LOCALITY_0)
+    {
+        switch(ordinal)
+        {
+            // We will not allow certain from locality 0 - aka the external TPM channel
+            case TPM_CC_ChangeEPS:
+            case TPM_CC_ChangePPS:
+            case TPM_CC_Clear:
+            case TPM_CC_ClearControl:
+            case TPM_CC_ClockSet:
+            case TPM_CC_PCR_Allocate:
+            case TPM_CC_PCR_SetAuthPolicy:
+            case TPM_CC_Shutdown:
+                return TIS_SESSION_RESULT_FILTERED;
+            default:
+                break;
+        }
+    }
+    
+    // Move on the and lets look at the handle filter
+    cursor += sizeof(ordinal);
+    
+    // Lookup the number of handles in the command
+    for(uint32_t n = 0; s_ccAttr[n].commandIndex != 0x0000 ;n++)
+    {
+        if(s_ccAttr[n].commandIndex == ordinal)
+        {
+            handleCount = s_ccAttr[n].cHandles;
+            break;
+        }
+    }
+    
+    // Easy elimination of invalid cases
+    if((handleCount < 0) || (handleCount > 3) ||
+       (cbCmd < (cursor + (sizeof(uint32_t) * handleCount))))
+    {
+        return TIS_SESSION_RESULT_FILTERED;
+    }
+    
+    // Read the handles from the command and see if they are allowed
+    for(uint32_t n = 0 ;n < handleCount; n++)
     {
-        delete m_SPITpmDev;
-        m_SPITpmDev = NULL;
+        uint32_t objectHandle = BE_BYTEARRAY_TO_UINT32(pbCmd, cursor);
+        cursor += sizeof(objectHandle);
+        
+        if(locality == TIS_LOCALITY_0)
+        {
+            switch(objectHandle)
+            {
+                // We will not allow the platform entity to be used from locality 0
+                case TPM_RH_PLATFORM:
+                    return TIS_SESSION_RESULT_FILTERED;
+                default:
+                    break;
+            }
+        }
     }
-    if(m_SPICSTpmDev != NULL)
+    
+    return TIS_SESSION_RESULT_COMPLETE;
+}
+#endif
+
+TIS_TPM20::TIS_RESULT
+TIS_TPM20::SendCommand(
+    uint8_t* pbCmd,
+    uint32_t cbCmd,
+    TIS_TPM20::TIS_LOCALITY locality)
+{
+    TIS_RESULT result = TIS_SESSION_RESULT_COMPLETE;
+    uint8_t tisStatus = 0;
+    uint16_t burstCount = 0;
+    uint32_t index = 0;
+
+    // Is the driver busy?
+    if(m_locality != TIS_NOT_IN_USE)
     {
-        delete m_SPICSTpmDev;
-        m_SPICSTpmDev = NULL;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.Schedule: TIS busy at locality %d\n\r", m_locality);
+#endif
+        // Can we preemt?
+        if(m_locality < locality)
+        {
+            result = TIS_SESSION_RESULT_OCCUPIED;
+            goto Cleanup;
+        }
+        else
+        {
+            result = TIS_SESSION_RESULT_PREEMPTED;
+            goto Cleanup;
+        }
+    }
+
+#ifndef TPM_TIS_NO_COMMAND_FILTERING
+    // Apply command filtering
+    if(ApplyFilter(locality, pbCmd, cbCmd) == TIS_SESSION_RESULT_FILTERED)
+    {
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.Schedule: Command filtered\n\r");
+#endif
+        result = TIS_SESSION_RESULT_FILTERED;
+        goto Cleanup;
+    }
+#endif
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Command: ");
+    for(uint32_t n = 0; n < cbCmd; n++) printf("%02x ", pbCmd[n]);
+    printf("\n\r");
+#endif
+
+    // Assert the locality
+
+    if(!RequestLocality(locality))
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+
+    // Make sure the TPM is ready for a command
+    if(!ReadRegister(TIS_STS_REGISTER, &tisStatus, sizeof(tisStatus)))
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+
+    // Get TPM ready
+    if((tisStatus & TIS_STS_COMMAND_READY) == 0)
+    {
+        if(!AbortCommand())
+        {
+            result = TIS_SESSION_RESULT_FAILED;
+            goto Cleanup;
+        }
     }
 #ifdef TPM_TIS_DEBUG_OUTPUT
-    printf("OK.\n\r");
+        printf("TIS.ReadyForCommand\n\r");
+#endif
+
+    // Submit Command
+    do
+    {
+        uint16_t iteration = 0;
+
+        if((burstCount = GetBurstCount()) == 0)
+        {
+            result = TIS_SESSION_RESULT_FAILED;
+            goto Cleanup;
+        }
+
+        // Assemble the buffer for transmission
+        iteration = min((cbCmd - index), min(burstCount, TIS_MAX_HW_FRAME_SIZE));
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.SendingBurst = %d\r\n", iteration);
 #endif
+        if(!WriteRegister(TIS_DATA_FIFO, &pbCmd[index], iteration))
+        {
+            result = TIS_SESSION_RESULT_FAILED;
+            goto Cleanup;
+        }
+
+        // Update the index
+        index += iteration;
+    } while((cbCmd - index) > 0);
+
+    // Command complete?
+    if((!ReadRegister(TIS_STS_REGISTER, &tisStatus, sizeof(tisStatus))) ||
+       (!(tisStatus & TIS_STS_VALID) || (tisStatus & TIS_STS_DATA_EXPECT)))
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.CommandComplete\n\r");
+#endif
+
+    // Arm the Interrupt
+    if(!TpmInteruptOn(TIS_INT_ENABLE_DATA_AVAILABLE_INT_ENABLE))
+    {
+        goto Cleanup;
+    }
+
+    // Kick the command off
+    tisStatus = TIS_STS_GO;
+    if(!WriteRegister(TIS_STS_REGISTER, &tisStatus, sizeof(tisStatus)))
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.Go.");
+#endif
+
+    result = TIS_SESSION_RESULT_COMPLETE;
+
+Cleanup:
+    if(result != TIS_SESSION_RESULT_COMPLETE)
+    {
+        AbortCommand();
+    }
+    return result;
 }
 
-uint32_t
-TISTPM20::Execute(
-    uint8_t locality,
-    uint8_t* pbCmd,
-    uint32_t cbCmd,
+TIS_TPM20::TIS_RESULT
+TIS_TPM20::RetrieveResponse(
     uint8_t* pbRsp,
     uint32_t cbRsp,
-    uint32_t timeout
+    uint32_t* pcbRsp
     )
 {
-    uint32_t result = 0;
-    Timeout watchdog;
+    TIS_RESULT result = TIS_SESSION_RESULT_COMPLETE;
+    uint8_t tisStatus = 0;
+    uint16_t burstCount = 0;
+    uint32_t index = 0;
+    uint32_t rspSize = 0;
 
-#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)
+    if(m_interrupt != 0)
     {
 #ifdef TPM_TIS_DEBUG_OUTPUT
         printf(".");
 #endif
-        wait_us(500);
+        result = TIS_SESSION_RESULT_PENDING;
+        return result;
     }
-    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);
+    printf("IRQ!\r\n");
 #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)
+    // Disarm the Interrupt
+    if(!TpmInteruptOn(0))
     {
-#ifdef TPM_TIS_DEBUG_OUTPUT
-        printf("TIS.Failed\n\r");
-#endif
         goto Cleanup;
     }
 
+    if((!ReadRegister(TIS_STS_REGISTER, &tisStatus, sizeof(tisStatus))) ||
+       (!(tisStatus & TIS_STS_VALID) || !(tisStatus & TIS_STS_DATA_AVAIL)))
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.DataAvailable\r\n");
+#endif
+
+    // Get the response header from the TPM
+    if(((burstCount = GetBurstCount()) == 0) ||
+       (burstCount < (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t))) ||
+       (!ReadRegister(TIS_DATA_FIFO, &pbRsp[index], (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t)))))
+    {
+        result = TIS_SESSION_RESULT_FAILED;
+        goto Cleanup;
+    }
+    index += (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t));
+    rspSize = BE_BYTEARRAY_TO_UINT32(pbRsp, sizeof(uint16_t));
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("Tis.ResponseSize = %d\r\n", rspSize);
+#endif
+
+    if(min(rspSize, cbRsp) > index)
+    {
+        do
+        {
+            uint16_t iteration = 0;
+
+            // Check to make sure the TPM has still data for us
+            if((!ReadRegister(TIS_STS_REGISTER, &tisStatus, sizeof(tisStatus))) ||
+               (!(tisStatus & TIS_STS_VALID) || !(tisStatus & TIS_STS_DATA_AVAIL))  ||
+               ((burstCount = GetBurstCount()) == 0))
+            {
+                result = TIS_SESSION_RESULT_FAILED;
+                goto Cleanup;
+            }
+            iteration = min((rspSize - index), min((cbRsp - index), min(burstCount, TIS_MAX_HW_FRAME_SIZE)));
+#ifdef TPM_TIS_DEBUG_OUTPUT
+            printf("TIS.ReceivingBurst = %d\r\n", iteration);
+#endif
+
+            // Read the data for this iteration
+            if(!ReadRegister(TIS_DATA_FIFO, &pbRsp[index], iteration))
+            {
+                goto Cleanup;
+            }
+            index += iteration;
+        }
+        while(index < min(rspSize, cbRsp));
+    }
+
+    *pcbRsp = index;
+
 #ifdef TPM_TIS_DEBUG_OUTPUT
     printf("TIS.Response: ");
-    for(uint32_t n = 0; n < result; n++) printf("%02x ", pbRsp[n]);
+    for(uint32_t n = 0; n < *pcbRsp; n++) printf("%02x ", pbRsp[n]);
     printf("\n\r");
 #endif
 
-#ifdef TPM_TIS_DEBUG_OUTPUT
-    printf("TIS.CancelTimeout\n\r");
-#endif
+    result = TIS_SESSION_RESULT_COMPLETE;
 
 Cleanup:
-    watchdog.detach();
-    m_ExclusiveAccess = false;
+    if((result != TIS_SESSION_RESULT_COMPLETE) && (result != TIS_SESSION_RESULT_PENDING))
+    {
+        AbortCommand();
+    }
+    // Release the locality again
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.ReleaseLocality\r\n");
+#endif
+    ReleaseLocality();
     return result;
 }
 
 uint32_t
-TISTPM20::ParseResponseHeader(
-    uint8_t* pbRsp,
-    uint32_t rspLen,
-    uint16_t* rspTag,
-    uint32_t* rspSize,
-    uint32_t* cursor
-    )
+TIS_TPM20::ParseResponseHeader(uint8_t* pbRsp, uint32_t cbRsp, uint16_t* rspTag, uint32_t* rspSize)
 {
     uint32_t rspResponseCode = 0;
+    uint32_t cursor = 0;
 
     // Check that the response header is well formatted
-    if(rspLen < (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t)))
+    if(cbRsp < (sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint32_t)))
     {
 #ifdef TPM_TIS_DEBUG_OUTPUT
-        printf("TIS.ResponseHdr.rspLen = 0x%08x\n\r", rspLen);
+        printf("TIS.ResponseHdr: Too short = 0x%08x\n\r", cbRsp);
 #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);
+    *rspTag = BE_BYTEARRAY_TO_UINT16(pbRsp, cursor);
+    cursor += sizeof(*rspTag);
+    *rspSize = BE_BYTEARRAY_TO_UINT32(pbRsp, cursor);
+    cursor += sizeof(*rspSize);
+    rspResponseCode = BE_BYTEARRAY_TO_UINT32(pbRsp, cursor);
+    cursor += sizeof(rspResponseCode);
 
     // Check the components
     if(((*rspTag != TPM_ST_NO_SESSIONS) && (*rspTag != TPM_ST_SESSIONS)) ||
-       (*rspSize != rspLen))
+       (*rspSize != cbRsp))
     {
 #ifdef TPM_TIS_DEBUG_OUTPUT
-        printf("TIS.ResponseHdr.rspTag = 0x%04x.rspLen=0x%08x\n\r", *rspTag, rspLen);
+        printf("TIS.ResponseHdr: Tag=0x%04x, Len=0x%08x, RC=0x%08x\n\r", *rspTag, *rspSize, rspResponseCode);
 #endif
         rspResponseCode = TPM_RC_FAILURE;
         goto Cleanup;
@@ -183,482 +434,10 @@
     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(
+TIS_TPM20::FullDuplex(
     bool readCycle,
-    uint8_t tisRegister,
+    uint16_t reg,
     uint8_t* pbBuffer,
     uint16_t cbBuffer
     )
@@ -668,207 +447,215 @@
     uint8_t dataByteOut;
 
     // Lock the bus for this operation
-    *m_SPICSTpmDev = 0;
+    m_chipSelect = 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");
+    uint32_t tisHdr = TIS_HEADER(m_locality, readCycle, reg, cbBuffer);
+#ifdef TPM_TIS_INTERFACE_DEBUG_OUTPUT
+    printf("TIS(LC:%d,RG:%02x,SZ:%02x,%s):", m_locality, reg, cbBuffer, (readCycle) ? "RD" : "WR");
 #endif
 
     for(uint8_t n = 0; n < sizeof(tisHdr); n++)
     {
-         dataByteOut = tisHdr >> (8 * (3 - n));
-         dataByteIn = m_SPITpmDev->write(dataByteOut);
+        dataByteOut = tisHdr >> (8 * (3 - n));
+        dataByteIn = m_spi.write(dataByteOut);
+//#ifdef TPM_TIS_INTERFACE_DEBUG_OUTPUT
+//        if(n < (sizeof(tisHdr) - 1)) printf("%02x ", dataByteOut);
+//        else printf("%02x", dataByteOut);
+//#endif
     }
-        
-    // 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++)
+
+    // The last bit we read full duplex is the first wait state indicator
+    int16_t waitCycleRetry = 100;
+    while(!(dataByteIn & 0x01))
     {
-        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");
+#ifdef TPM_TIS_INTERFACE_DEBUG_OUTPUT
+        printf(".");
 #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))
+        // Read the next byte to see is we still have to wait
+        if((dataByteIn = m_spi.write(0x00)) == 0x01)
         {
             break;
         }
 
-        // Request access
-        dataByte = TIS_ACCESS_REQUEST_USE;
-        if(!SPIFullDuplex(false, TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
-        {
-            return false;
-        }
-        if(m_TimeoutTriggered)
+        // Check the timeout
+        if(waitCycleRetry-- <= 0)
         {
-            // We give up eventually
-            return false;
+            result = false;
+            goto Cleanup;
         }
-        wait_us(10);
     }
-    while(1);
-    return true;
+
+    // Full duplex the payload
+    for(uint8_t n = 0; n < cbBuffer; n++)
+    {
+        dataByteOut = (readCycle) ? 0x00 : pbBuffer[n];
+        dataByteIn = m_spi.write(dataByteOut);
+        if(readCycle) pbBuffer[n] = dataByteIn;
+#ifdef TPM_TIS_INTERFACE_DEBUG_OUTPUT
+        printf("%02x ", (readCycle) ? dataByteIn : dataByteOut);
+#endif
+    }
+    result = true;
+
+Cleanup:
+#ifdef TPM_TIS_INTERFACE_DEBUG_OUTPUT
+    printf("\r\n");
+#endif
+    // Make sure to release the bus before we leave
+    m_chipSelect = 1;
+    return result;
 }
 
-bool TISTPM20::ReleaseLocality()
+uint16_t
+TIS_TPM20::GetBurstCount()
 {
-    do
+    if(m_fixedBurstCount != 0)
     {
-        // Do we even have access?
-        uint8_t dataByte = 0xFF;
-        if(!SPIFullDuplex(true, TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+        return m_fixedBurstCount;
+    }
+    else
+    {
+        uint8_t dataBytes[sizeof(uint16_t)] = {0};
+        if(!ReadRegister(TIS_STS_BURSTCOUNT_REGISTER, dataBytes, sizeof(dataBytes)))
         {
-            return false;
-        }
-        if(!(dataByte & TIS_ACCESS_ACTIVE_LOCALITY) &&
-            (dataByte & TIS_ACCESS_VALID))
-        {
-            break;
+            return 0;
         }
-
-        // 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);
+#ifdef TPM_TIS_DEBUG_OUTPUT
+        printf("TIS.BurstCount = %d\r\n", LE_BYTEARRAY_TO_UINT16(dataBytes , 0));
+#endif
+        return LE_BYTEARRAY_TO_UINT16(dataBytes , 0);
     }
-    while(1);
-    return true;
 }
 
 bool
-TISTPM20::GetBurstCount(
-    uint16_t* pBurstCount
-    )
+TIS_TPM20::RequestLocality(TIS_TPM20::TIS_LOCALITY locality)
 {
-    uint8_t dataBytes[sizeof(*pBurstCount)] = {0};
-    do
+    m_locality = locality;
+    
+    for(uint8_t n = 0; n < 100; n++)
     {
-        if(!SPIFullDuplex(true, TIS_BURSTCOUNT_REGISTER, dataBytes, sizeof(dataBytes)))
+        uint8_t dataByte = 0;
+        // Read a valid access register. Turns out, it may take a couple of times if the TPM was sleeping
+        do
         {
-            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;
+            dataByte = 0;
+            if(!ReadRegister(TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+            {
+                goto Cleanup;
+            }
+            
+            // First time we hit that, the TPM has to wake up give it some time
+            if(!(dataByte & TIS_ACCESS_VALID))
+            {
+                wait_us(5000);
+            }
         }
-        if(m_TimeoutTriggered)
+        while(!(dataByte & TIS_ACCESS_VALID));
+
+        // If we have the locality we are done
+        if(dataByte & TIS_ACCESS_ACTIVE_LOCALITY)
         {
-            // We give up eventually
-            return false;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+            printf("TIS.LocalityAquired\n\r");
+#endif
+            return true;
         }
-        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;
+        // Request the locality
+        dataByte = TIS_ACCESS_REQUEST_USE;
+        if(!WriteRegister(TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+        {
+            goto Cleanup;
+        }
+    }
+
+    m_locality = TIS_NOT_IN_USE;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.LocalityRequest = FAILED\n\r");
+#endif
+Cleanup:
+    return false;
 }
 
 bool
-TISTPM20::WaitForStatus(
-    uint8_t statusMask
-    )
+TIS_TPM20::ReleaseLocality()
 {
-    bool result = false;
-    uint8_t tisStatus = 0xFF;
+    for(uint8_t n = 0; n < 100; n++)
+    {
+        uint8_t dataByte = 0;
+        // Read a valid access register. Turns out, it may take a couple of times if the TPM was sleeping
+        do
+        {
+            if(!ReadRegister(TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
+            {
+                break;
+            }
+ 
+            // First time we hit that, the TPM has to wake up give it some time
+            if(!(dataByte & TIS_ACCESS_VALID))
+            {
+                wait_us(5000);
+            }
+        }
+        while(!(dataByte & TIS_ACCESS_VALID));
 
-    do
-    {
-        if(!GetStatus(&tisStatus))
+        // If we don't have the locality we are done
+        if(!(dataByte & TIS_ACCESS_ACTIVE_LOCALITY))
         {
-            goto Cleanup;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+            printf("TIS.LocalityReleased\n\r");
+#endif
+            m_locality = TIS_NOT_IN_USE;
+            return true;
         }
-        if((tisStatus & statusMask) == statusMask)
+
+        // Drop the locality
+        dataByte = TIS_ACCESS_ACTIVE_LOCALITY;
+        if(!WriteRegister(TIS_ACCESS_REGISTER, &dataByte, sizeof(dataByte)))
         {
-            result = true;
             break;
         }
-        wait_us(10);
     }
-    while(!m_TimeoutTriggered);
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.LocalityReleased = FAILED\n\r");
+#endif
 
-Cleanup:    
-    return result;
+    return false;
 }
 
 bool
-TISTPM20::Abort()
+TIS_TPM20::TpmInteruptOn(uint32_t intEnable)
 {
-    uint8_t dataByte = TIS_STS_COMMAND_READY;
-    return SPIFullDuplex(false, TIS_STATUS_REGISTER, &dataByte, sizeof(dataByte));
+    uint32_t int_enable = TIS_INT_ENABLE_TYPE_POLARITY_LOW;
+    uint8_t dataRegister[sizeof(uint32_t)] = {0};
+    uint8_t dataByte = 0;
+
+    if(intEnable != 0)
+    {
+        int_enable |= (uint32_t)TIS_INT_ENABLE_GLOBAL_INT_ENABLE | intEnable;
+    }
+
+    // Read the Interrupt state
+    ReadRegister(TIS_INT_STATUS_REGISTER, &dataByte, sizeof(dataByte));
+
+    dataByte = TIS_INT_STATUS_RESET_ALL;
+    LE_UINT32_TO_BYTEARRAY(int_enable, dataRegister, 0);
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.ArmTpmInterupt = 0x%08x\n\r", intEnable);
+#endif
+    return ((WriteRegister(TIS_INT_STATUS_REGISTER, &dataByte, sizeof(dataByte))) &&
+            (WriteRegister(TIS_INT_ENABLE_REGISTER, dataRegister, sizeof(dataRegister))));
 }
 
 bool
-TISTPM20::Go()
+TIS_TPM20::AbortCommand()
 {
-    uint8_t dataByte = TIS_STS_GO;
-    return SPIFullDuplex(false, TIS_STATUS_REGISTER, &dataByte, sizeof(dataByte));
-}
\ No newline at end of file
+    uint8_t tisStatus = TIS_STS_COMMAND_READY;
+#ifdef TPM_TIS_DEBUG_OUTPUT
+    printf("TIS.AbortCommand\n\r");
+#endif
+    return((WriteRegister(TIS_STS_REGISTER, &tisStatus, sizeof(tisStatus))) &&
+           (ReadRegister(TIS_STS_REGISTER, &tisStatus, sizeof(tisStatus))) &&
+           ((tisStatus & TIS_STS_COMMAND_READY) != 0));
+}