xen/tools/vtpm/vtpm.patch

diff -uprN tpm_emulator/AUTHORS vtpm/AUTHORS
--- tpm_emulator/AUTHORS	2006-12-08 12:51:29.000000000 -0800
+++ vtpm/AUTHORS	2006-12-13 16:38:52.000000000 -0800
@@ -1,3 +1,3 @@
 Mario Strasser <mast@gmx.net>
 Heiko Stamer <stamer@gaos.org> [DAA]
-INTEL Corp <> [Dropped to Ring3]
+INTEL Corp <> [VTPM Extensions]
diff -uprN tpm_emulator/ChangeLog vtpm/ChangeLog
--- tpm_emulator/ChangeLog	2006-12-08 12:51:29.000000000 -0800
+++ vtpm/ChangeLog	2006-12-13 16:38:52.000000000 -0800
@@ -1,5 +1,6 @@
 ????-??-?? Intel Corp
 	* Moved module out of kernel to run as a ring 3 app
+	* Modified save_to_file and load_from_file to call xen VTPM manager
 
 2006-06-23  Mario Strasser <mast@gmx.net>
 	* tpm_startup.c: behaviour of ST_CLEAR and storage of
diff -uprN tpm_emulator/linux_module.h vtpm/linux_module.h
--- tpm_emulator/linux_module.h	2006-12-08 12:51:29.000000000 -0800
+++ vtpm/linux_module.h	2007-01-09 14:49:06.000000000 -0800
@@ -44,18 +44,26 @@
 #define TPM_DEVICE_NAME   "tpm"
 #define TPM_MODULE_NAME   "tpm_emulator"
 
+/* debug and log output functions */
+extern int dmi_id; 
+
 #ifdef DEBUG
-#define debug(fmt, ...) printf("TPMD: %s:%d: Debug: " fmt "\n", \
-                        __FILE__, __LINE__, ## __VA_ARGS__)
+#define debug(fmt, ...) printf("TPMD[%d]: %s:%d: Debug: " fmt "\n", \
+                        dmi_id, __FILE__, __LINE__, ## __VA_ARGS__)
+#define debug_nostop(fmt, ...) printf("TPMD[%d]: %s:%d: Debug: " fmt, \
+                        dmi_id, __FILE__, __LINE__, ## __VA_ARGS__)
+#define debug_more(fmt, ...) printf( fmt, ## __VA_ARGS__ )
 #else
 #define debug(fmt, ...) 
+#define debug_nostop(fmt, ...) 
+#define debug_more(fmt, ...)
 #endif
-#define info(fmt, ...)  printf("TPMD: %s:%d: Info: " fmt "\n", \
-                        __FILE__, __LINE__, ## __VA_ARGS__)
-#define error(fmt, ...) printf("TPMD: %s:%d: Error: " fmt "\n", \
-                        __FILE__, __LINE__, ## __VA_ARGS__)
-#define alert(fmt, ...) printf("TPMD: %s:%d: Alert: " fmt "\n", \
-                        __FILE__, __LINE__, ## __VA_ARGS__)
+#define info(fmt, ...)  printf("TPMD[%d]: %s:%d: Info: " fmt "\n", \
+                        dmi_id, __FILE__, __LINE__, ## __VA_ARGS__)
+#define error(fmt, ...) printf("TPMD[%d]: %s:%d: Error: " fmt "\n", \
+                        dmi_id, __FILE__, __LINE__, ## __VA_ARGS__)
+#define alert(fmt, ...) printf("TPMD[%d]: %s:%d: Alert: " fmt "\n", \
+                        dmi_id, __FILE__, __LINE__, ## __VA_ARGS__)
 
 /* memory allocation */
 
diff -uprN tpm_emulator/Makefile vtpm/Makefile
--- tpm_emulator/Makefile	2006-12-08 12:51:29.000000000 -0800
+++ vtpm/Makefile	2006-12-13 16:38:52.000000000 -0800
@@ -7,7 +7,7 @@
 COMPILE_ARCH    ?= $(shell uname -m | sed -e s/i.86/x86_32/)
 
 # module settings
-BIN            := tpm_emulator
+BIN            := vtpmd
 VERSION_MAJOR  := 0
 VERSION_MINOR  := 4
 VERSION_BUILD  := $(shell date +"%s")
@@ -22,7 +22,7 @@ TOOLS_INSTALL_DIR = $(DESTDIR)/usr/bin
 
 CC      := gcc
 CFLAGS  += -g -Wall $(INCLUDE) -DDEBUG
-CFLAGS  += -I. -Itpm
+CFLAGS  += -I. -Itpm -I../../vtpm_manager/manager
 
 # Is the simulator running in it's own vm?
 #CFLAGS += -DVTPM_MULTI_VM
@@ -62,7 +62,6 @@ $(BIN):	$(src)/crypto/gmp.h $(src)/crypt
 
 install: $(BIN)
 	$(INSTALL_PROG) $(BIN) $(TOOLS_INSTALL_DIR)
-	@if [ ! -d "/var/tpm" ]; then mkdir /var/tpm; fi
 
 clean:
 	rm -f $(src)/crypto/gmp.h $(src)/crypto/libgmp.a $(OBJS)
@@ -98,3 +97,4 @@ version:
 	@echo "#endif /* _TPM_VERSION_H_ */" >> $(src)/tpm_version.h
 
 .PHONY: all install clean dist gmp version
+
diff -uprN tpm_emulator/tpm/tpm_capability.c vtpm/tpm/tpm_capability.c
--- tpm_emulator/tpm/tpm_capability.c	2006-06-23 03:37:07.000000000 -0700
+++ vtpm/tpm/tpm_capability.c	2007-01-10 10:00:49.000000000 -0800
@@ -136,8 +136,18 @@ static TPM_RESULT cap_property(UINT32 su
 
     case TPM_CAP_PROP_TIS_TIMEOUT:
       debug("[TPM_CAP_PROP_TIS_TIMEOUT]");
-      /* TODO: TPM_CAP_PROP_TIS_TIMEOUT */
-      return TPM_FAIL;
+      /* TODO: TPM_CAP_PROP_TIS_TIMEOUT: Measure these values and determine correct ones */
+      UINT32 len = *respSize = 16;
+      BYTE *ptr = *resp = tpm_malloc(*respSize);
+      if (ptr == NULL || 
+          tpm_marshal_UINT32(&ptr, &len, 200000) ||
+          tpm_marshal_UINT32(&ptr, &len, 200000) ||
+          tpm_marshal_UINT32(&ptr, &len, 200000) ||
+          tpm_marshal_UINT32(&ptr, &len, 200000)) {
+        tpm_free(*resp);
+        return TPM_FAIL;
+      }
+      return TPM_SUCCESS;
 
     case TPM_CAP_PROP_STARTUP_EFFECT:
       debug("[TPM_CAP_PROP_STARTUP_EFFECT]");
@@ -190,7 +200,11 @@ static TPM_RESULT cap_property(UINT32 su
 
     case TPM_CAP_PROP_DURATION:
       debug("[TPM_CAP_PROP_DURATION]");
-      /* TODO: TPM_CAP_PROP_DURATION */
+      /* TODO: TPM_CAP_PROP_DURATION: Measure these values and return accurate ones */
+      BYTE dur[]= {0x0,0x0,0x0,0xc,0x0,0x7,0xa1,0x20,0x0,0x1e,0x84,0x80,0x11,0xe1,0xa3,0x0}; 
+      *respSize = 16;
+      *resp = tpm_malloc(*respSize);
+      memcpy(*resp,dur,16); 
       return TPM_FAIL;
 
     case TPM_CAP_PROP_ACTIVE_COUNTER:
diff -uprN tpm_emulator/tpm/tpm_data.c vtpm/tpm/tpm_data.c
--- tpm_emulator/tpm/tpm_data.c	2006-12-08 12:51:29.000000000 -0800
+++ vtpm/tpm/tpm_data.c	2006-12-13 16:38:52.000000000 -0800
@@ -1,6 +1,7 @@
 /* Software-Based Trusted Platform Module (TPM) Emulator for Linux
  * Copyright (C) 2004 Mario Strasser <mast@gmx.net>,
  *                    Swiss Federal Institute of Technology (ETH) Zurich
+ * Copyright (C) 2005 INTEL Corp
  *
  * This module is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published
@@ -15,10 +16,15 @@
  * $Id: tpm_data.c 98 2006-05-07 14:16:29Z hstamer $
  */
 
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
 #include "tpm_emulator.h"
 #include "tpm_structures.h"
 #include "tpm_marshalling.h"
-#include "linux_module.h"
+#include "vtpm_manager.h"
 
 TPM_DATA tpmData;
 
@@ -158,45 +164,232 @@ void tpm_release_data(void)
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
-#include <unistd.h>
 
-#define TPM_STORAGE_FILE "/var/tpm/tpm_emulator-1.2." STR(VERSION_MAJOR) "." STR(VERSION_MINOR) 
+ static int vtpm_tx_fh=-1, vtpm_rx_fh=-1;
+
+#ifdef VTPM_MUTLI_VM
+ #define DEV_FE "/dev/tpm"
+#else
+ #define VTPM_RX_FIFO_D  "/var/vtpm/fifos/vtpm_rsp_to_%d.fifo"
+ #define VTPM_TX_FIFO  "/var/vtpm/fifos/vtpm_cmd_from_all.fifo"
+
+ extern int dmi_id;
+ static char *vtpm_rx_name=NULL; 
+#endif
 
 static int write_to_file(uint8_t *data, size_t data_length)
 {
-  int res;
-  int fp;
-  fp = open(TPM_STORAGE_FILE, O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR | S_IWUSR);
-  res = write(fp, data, data_length);
-  close(fp);
-  return (res == data_length) ? 0 : -1;
+  int res, out_data_size, in_header_size;
+  BYTE *ptr, *out_data, *in_header;
+  UINT32 result, len, in_rsp_size;
+  UINT16 tag = VTPM_TAG_REQ;
+	
+  printf("Saving NVM\n");
+  if (vtpm_tx_fh < 0) {
+#ifdef VTPM_MUTLI_VM
+    vtpm_tx_fh = open(DEV_FE, O_RDWR);
+#else
+	vtpm_tx_fh = open(VTPM_TX_FIFO, O_WRONLY);
+#endif
+  }
+
+  if (vtpm_tx_fh < 0) {
+		return -1;
+  }
+ 
+  // Send request to VTPM Manager to encrypt data
+#ifdef VTPM_MUTLI_VM
+  out_data_size = len = VTPM_COMMAND_HEADER_SIZE_CLT + data_length;
+#else
+  out_data_size = len = VTPM_COMMAND_HEADER_SIZE_SRV + data_length;
+#endif
+  
+  out_data = ptr = (BYTE *) malloc(len);
+
+  if (ptr == NULL
+#ifndef VTPM_MUTLI_VM
+      || tpm_marshal_UINT32(&ptr, &len, dmi_id)
+#endif
+	  || tpm_marshal_UINT16(&ptr, &len, tag)
+#ifdef VTPM_MUTLI_VM
+	  || tpm_marshal_UINT32(&ptr, &len, out_data_size)
+#else
+	  || tpm_marshal_UINT32(&ptr, &len, out_data_size - sizeof(uint32_t))
+#endif  
+	  || tpm_marshal_UINT32(&ptr, &len, VTPM_ORD_SAVENVM)
+	  || tpm_marshal_BYTE_ARRAY(&ptr, &len, data, data_length)) {
+	free(out_data);
+	return -1;
+  }
+  
+  printf("\tSending SaveNVM Command.\n");
+  res = write(vtpm_tx_fh, out_data, out_data_size);
+  free(out_data);
+  if (res != out_data_size) return -1;
+
+  if (vtpm_rx_fh < 0) {
+#ifdef VTPM_MUTLI_VM
+    vtpm_rx_fh = vtpm_tx_fh
+#else
+    if (vtpm_rx_name == NULL) {
+      vtpm_rx_name = malloc(10 + strlen(VTPM_RX_FIFO_D));
+      sprintf(vtpm_rx_name, VTPM_RX_FIFO_D, (uint32_t) dmi_id);
+    }
+	vtpm_rx_fh = open(vtpm_rx_name, O_RDONLY);
+#endif
+  }
+
+  if (vtpm_rx_fh < 0) {
+		return -1;
+  }
+  
+  // Read Header of response so we can get the size & status
+#ifdef VTPM_MUTLI_VM
+  in_header_size = len = VTPM_COMMAND_HEADER_SIZE_CLT;
+#else
+  in_header_size = len = VTPM_COMMAND_HEADER_SIZE_SRV;
+#endif
+  in_header = ptr = malloc(in_header_size);
+  
+  printf("\tReading SaveNVM header.\n");
+  res = read(vtpm_rx_fh, in_header, in_header_size);
+
+  if ( (res != in_header_size)
+#ifndef VTPM_MUTLI_VM
+       || tpm_unmarshal_UINT32(&ptr, &len, (UINT32*)&dmi_id)
+#endif
+	   || tpm_unmarshal_UINT16(&ptr, &len, &tag)
+	   || tpm_unmarshal_UINT32(&ptr, &len, &in_rsp_size)
+	   || tpm_unmarshal_UINT32(&ptr, &len, &result) ) {
+	  free(in_header);
+	  return -1;
+  }
+  free(in_header);
+  
+  if (result != VTPM_SUCCESS) {
+      return -1;  
+  }
+
+#ifdef VTPM_MUTLI_VM
+  close(vtpm_tx_fh); close(vtpm_rx_fh);
+#endif
+	  
+  printf("\tFinishing up SaveNVM\n");
+  return (0);
 }
 
 static int read_from_file(uint8_t **data, size_t *data_length)
 {
-  int res;
-  int fp, file_status;
-  struct stat file_info;
-  fp = open(TPM_STORAGE_FILE, O_RDONLY, 0);
-  file_status = fstat(fp, &file_info);
-  if (file_status < 0) {
-    close(fp);
-    return -1;
-  } 
+  int res, out_data_size, in_header_size;
+  uint8_t *ptr, *out_data, *in_header;
+  UINT16 tag = VTPM_TAG_REQ;
+  UINT32 len, in_rsp_size, result;
+#ifdef VTPM_MUTLI_VM
+	int vtpm_rx_fh, vtpm_tx_fh;
+#endif
+	
+  printf("Loading NVM.\n");
+  if (vtpm_tx_fh < 0) {
+#ifdef VTPM_MUTLI_VM
+    vtpm_tx_fh = open(DEV_FE, O_RDWR);
+#else
+	vtpm_tx_fh = open(VTPM_TX_FIFO, O_WRONLY);
+#endif
+  }
 
-  *data_length = file_info.st_size; 
-  *data = tpm_malloc(*data_length);
-  if (*data == NULL) {
-    close(fp);
+  if (vtpm_tx_fh < 0) {
+		return -1;
+  }
+ 
+  // Send request to VTPM Manager to encrypt data
+#ifdef VTPM_MUTLI_VM
+  out_data_size = len = VTPM_COMMAND_HEADER_SIZE_CLT;
+#else
+  out_data_size = len = VTPM_COMMAND_HEADER_SIZE_SRV;
+#endif
+  out_data = ptr = (BYTE *) malloc(len);
+
+  if (ptr == NULL
+#ifndef VTPM_MUTLI_VM
+      || tpm_marshal_UINT32(&ptr, &len, dmi_id)
+#endif  
+      || tpm_marshal_UINT16(&ptr, &len, tag)
+#ifdef VTPM_MUTLI_VM
+      || tpm_marshal_UINT32(&ptr, &len, out_data_size)
+#else
+      || tpm_marshal_UINT32(&ptr, &len, out_data_size - sizeof(uint32_t))
+#endif
+      || tpm_marshal_UINT32(&ptr, &len, VTPM_ORD_LOADNVM)) {
+    free(out_data);
     return -1;
   }
-  res = read(fp, *data, *data_length);
-  close(fp);
+
+  printf("\tSending LoadNVM command\n");
+  res = write(vtpm_tx_fh, out_data, out_data_size);
+  free(out_data);
+  if (res != out_data_size) return -1;
+
+    if (vtpm_rx_fh < 0) {
+#ifdef VTPM_MUTLI_VM
+    vtpm_rx_fh = vtpm_tx_fh;
+#else
+    if (vtpm_rx_name == NULL) {
+      vtpm_rx_name = malloc(10 + strlen(VTPM_RX_FIFO_D));
+      sprintf(vtpm_rx_name, VTPM_RX_FIFO_D, (uint32_t) dmi_id);
+    }
+	vtpm_rx_fh = open(vtpm_rx_name, O_RDONLY);
+#endif
+  }
+
+  if (vtpm_rx_fh < 0) {
+		return -1;
+  }
+  
+  // Read Header of response so we can get the size & status
+#ifdef VTPM_MUTLI_VM
+  in_header_size = len = VTPM_COMMAND_HEADER_SIZE_CLT;
+#else
+  in_header_size = len = VTPM_COMMAND_HEADER_SIZE_SRV;
+#endif
+  in_header = ptr = malloc(in_header_size);
+  
+  printf("\tReading LoadNVM header\n");
+  res = read(vtpm_rx_fh, in_header, in_header_size);
+
+  if ( (res != in_header_size)
+#ifndef VTPM_MUTLI_VM
+       || tpm_unmarshal_UINT32(&ptr, &len, (UINT32*)&dmi_id)
+#endif
+       || tpm_unmarshal_UINT16(&ptr, &len, &tag)
+       || tpm_unmarshal_UINT32(&ptr, &len, &in_rsp_size)
+       || tpm_unmarshal_UINT32(&ptr, &len, &result) ) {
+      free(in_header);
+      return -1;
+  }
+  free(in_header);
+  
+  if (result != VTPM_SUCCESS) {
+      return -1;  
+  }
+
+  // Read Encrypted data from VTPM Manager
+  *data_length = in_rsp_size - VTPM_COMMAND_HEADER_SIZE_CLT;
+  *data = (uint8_t *) malloc(*data_length);
+
+  printf("\tReading clear data from LoadNVM.\n");
+  res = read(vtpm_rx_fh, *data, *data_length);
+#ifdef VTPM_MUTLI_VM
+  close(vtpm_rx_fh);close(vtpm_tx_fh);
+#endif 
+	
+  printf("\tReturing from loading NVM\n");
   if (res != *data_length) {
-    tpm_free(*data);
-    return -1;
+      free(*data);
+      return -1;
+  } else {
+      return 0;
   }
-  return 0;
+
 }
 
 #else
diff -uprN tpm_emulator/tpmd.c vtpm/tpmd.c
--- tpm_emulator/tpmd.c	2006-12-08 12:51:29.000000000 -0800
+++ vtpm/tpmd.c	2007-01-09 14:48:56.000000000 -0800
@@ -21,12 +21,24 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <sys/time.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <errno.h>
 
 #include "tpm_emulator.h"
+#include "vtpm_manager.h"
 
-#define TPM_RX_FNAME "/var/tpm/tpm_in.fifo"
-#define TPM_TX_FNAME "/var/tpm/tpm_out.fifo"
+#ifdef VTPM_MULTI_VM
+ #define DEV_BE "/dev/vtpm"
+#else
+ #define PVM_RX_FIFO_D "/var/vtpm/fifos/tpm_cmd_to_%d.fifo"
+ #define PVM_TX_FIFO "/var/vtpm/fifos/tpm_rsp_from_all.fifo"
 
+ #define HVM_RX_FIFO_D "/var/vtpm/socks/%d.socket"
+#endif
+
+ int dmi_id;
+						
 #define BUFFER_SIZE 2048
 
 static int devurandom=0;
@@ -38,7 +50,7 @@ void get_random_bytes(void *buf, int nby
   }
 
   if (read(devurandom, buf, nbytes) != nbytes) {
-      printf("Can't get random number.\n");
+      error("Can't get random number.\n");
       exit(-1);
   }
 }
@@ -52,105 +64,182 @@ uint64_t tpm_get_ticks(void)
 
 int main(int argc, char **argv)
 {
-  uint8_t in[BUFFER_SIZE], *out;
+  uint8_t type, in[BUFFER_SIZE], *out, *addressed_out;
+  char *vtpm_rx_file=NULL;
   uint32_t out_size;
   int in_size, written;
-  int i;
-  struct stat file_info;
+  int i, guest_id=-1;
 
-  int tpm_tx_fh=-1, tpm_rx_fh=-1;
+#ifndef VTPM_MULTI_VM
+  int sockfd = -1;
+  struct sockaddr_un addr;
+  struct sockaddr_un client_addr;
+  unsigned int client_length;
+
+#endif
+ 
+  int vtpm_tx_fh=-1, vtpm_rx_fh=-1;
+#ifdef VTPM_MULTI_VM
   if (argc < 2) {
-    printf("Usage: tpmd clear|save|deactivated\n" );
+    error("Usage: tpmd clear|save|deactivated\n" );
+#else
+  if (argc < 4) {
+    error("Usage: tpmd clear|save|deactivated pvm|hvm vtpmid\n" );
+#endif
 	  return -1;
   }
 
+#ifndef VTPM_MULTI_VM
+  /* setup type of vm */
+  if (!strcmp(argv[2], "pvm")) {
+    type = VTPM_TYPE_PVM; // Get commands from vTPM Manager through fifo
+  } else if (!strcmp(argv[2], "hvm")) {
+    type = VTPM_TYPE_HVM; // Get commands from qemu via socket
+  } else {
+    error("invalid vTPM type '%s'.\n", argv[2]);
+  }
+
+  dmi_id = atoi(argv[3]);
+
+  if (type == VTPM_TYPE_PVM) {
+    vtpm_rx_file = malloc(10 + strlen(PVM_RX_FIFO_D));
+    sprintf(vtpm_rx_file, PVM_RX_FIFO_D, (uint32_t) dmi_id);
+  } else {
+    vtpm_rx_file = malloc(10 + strlen(HVM_RX_FIFO_D));
+    sprintf(vtpm_rx_file, HVM_RX_FIFO_D, (uint32_t) dmi_id);
+
+    if ( (sockfd = socket(PF_UNIX,SOCK_STREAM,0)) < 0) {
+          error("Unable to create socket. errno = %d\n", errno);
+      exit (-1);
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.sun_family = AF_UNIX;
+    strcpy(addr.sun_path,vtpm_rx_file );
+    unlink(addr.sun_path);
+  }
+#endif
+
+#ifdef VTPM_MULTI_VM
+  info("Initializing tpm state: %s\n", argv[1]);
+#else
+  info("Initializing tpm state: %s, type: %s, id: %d\n", argv[1], argv[2], dmi_id);
+#endif
+
   /* initialize TPM emulator */
   if (!strcmp(argv[1], "clear")) {
-    printf("Initializing tpm: %s\n", argv[1]);
     tpm_emulator_init(1);
-  } else if (!strcmp(argv[1], "save")) { 
-    printf("Initializing tpm: %s\n", argv[1]);
+  } else if (!strcmp(argv[1], "save")) {
     tpm_emulator_init(2);
   } else if (!strcmp(argv[1], "deactivated")) {
-    printf("Initializing tpm: %s\n", argv[1]);
     tpm_emulator_init(3);
   } else {
-    printf("invalid startup mode '%s'; must be 'clear', "
+    error("invalid startup mode '%s'; must be 'clear', "
       "'save' (default) or 'deactivated", argv[1]);
     return -1;
   }
-
-  if ( stat(TPM_RX_FNAME, &file_info) == -1) {
-    if ( mkfifo(TPM_RX_FNAME, S_IWUSR | S_IRUSR ) ) {
-      printf("Failed to create fifo %s.\n", TPM_RX_FNAME);
-      return -1;
-    }
-  }
-
-  if ( stat(TPM_TX_FNAME, &file_info) == -1) {
-    if ( mkfifo(TPM_TX_FNAME, S_IWUSR | S_IRUSR ) ) {
-      printf("Failed to create fifo %s.\n", TPM_TX_FNAME);
-      return -1;
-    }
-  }
-
+  
   while (1) {
 abort_command:
-    if (tpm_rx_fh < 0) {
-      tpm_rx_fh = open(TPM_RX_FNAME, O_RDONLY);
+    if (vtpm_rx_fh < 0) {
+#ifdef VTPM_MUTLI_VM
+      vtpm_rx_fh = open(DEV_BE, O_RDWR);
+#else
+      if (type == VTPM_TYPE_PVM) {
+        vtpm_rx_fh = open(vtpm_rx_file, O_RDONLY);
+      } else {
+        if (bind(sockfd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+          error("Unable to bind(). errno = %d\n", errno);
+          exit (-1);
+        }
+
+        if (listen(sockfd, 10) <0) {
+          error("Unable to listen(). errno = %d\n", errno);
+          exit (-1);
+        }
+
+        memset(&client_addr, 0, sizeof(client_addr));
+        client_length = sizeof(client_addr);
+
+        vtpm_rx_fh = vtpm_tx_fh = accept(sockfd, &client_addr, &client_length);
+      }
+#endif
     }
     
-    if (tpm_rx_fh < 0) {
-      printf("ERROR: failed to open devices to listen to guest.\n");
+    if (vtpm_rx_fh < 0) {
+      error("Failed to open devices to listen to guest.\n");
       return -1;
     }
     
-    if (tpm_tx_fh < 0) {
-      tpm_tx_fh = open(TPM_TX_FNAME, O_WRONLY);
-    }
-
-    if (tpm_tx_fh < 0) {
-      printf("ERROR: failed to open devices to respond to guest.\n");
-      return -1;
-    }
-
-    in_size = read(tpm_rx_fh, in, BUFFER_SIZE);
+    in_size = read(vtpm_rx_fh, in, BUFFER_SIZE);
     if (in_size < 6) { // Magic size of minium TPM command
-      printf("Recv[%d] to small: 0x", in_size);
+      info("Recv incomplete command of %d bytes.", in_size);
       if (in_size <= 0) {
-          close(tpm_rx_fh);
-          tpm_rx_fh = -1;
+          close(vtpm_rx_fh);
+          vtpm_rx_fh = -1;
           goto abort_command;
       }
     } else { 
-      printf("Recv[%d]: 0x", in_size);
+      debug_nostop("Recv[%d]: 0x", in_size);
       for (i=0; i< in_size; i++) 
-        printf("%x ", in[i]);
-      printf("\n");
+        debug_more("%x ", in[i]);
+      debug_more("\n");
     }
 
-    
-    if (tpm_handle_command(in, in_size, &out, &out_size) != 0) { 
-        printf("ERROR: Handler Failed.\n");
+    if (guest_id == -1) {
+        guest_id = *((uint32_t *) in);
+    } else {
+        if (guest_id != *((uint32_t *) in) ) {
+            error("WARNING: More than one guest attached\n");
+        }
+    }
+
+    if (vtpm_tx_fh < 0) {
+#ifdef VTPM_MUTLI_VM
+      vtpm_tx_fh = open(DEV_BE, O_RDWR);
+      vtpm_rx_fh = vtpm_tx_fh;
+#else
+      if (type == VTPM_TYPE_PVM) {
+        vtpm_tx_fh = open(PVM_TX_FIFO, O_WRONLY);
+      } // No need to open the other direction for HVM
+#endif
+    }
+
+    if (vtpm_tx_fh < 0) {
+      error("Failed to open devices to respond to guest.\n");
+      return -1;
+    }
+
+    // Handle the command, but skip the domain id header    
+    if (tpm_handle_command(in + sizeof(uint32_t), in_size - sizeof(uint32_t), &out, &out_size) != 0) { 
+      error("Handler Failed.\n");
     }
 
-    written = write(tpm_tx_fh, out, out_size);
+    addressed_out = (uint8_t *) tpm_malloc(sizeof(uint32_t) + out_size);
+    *(uint32_t *) addressed_out = *(uint32_t *) in;
+    memcpy(addressed_out + sizeof(uint32_t), out, out_size);
+
+    written = write(vtpm_tx_fh, addressed_out, out_size + sizeof(uint32_t));
 
-    if (written != out_size ) {
-      printf("ERROR: Part of response not written %d/%d.\nAttempt: ", written, out_size);
+    if (written != out_size + sizeof(uint32_t)) {
+      error("Part of response not written %d/%d.\n", written, out_size);
     } else {
-      printf("Sent[%Zu]: ", out_size);
+      debug_nostop("Sent[%Zu]: ", out_size + sizeof(uint32_t));
+      for (i=0; i< out_size+ sizeof(uint32_t); i++)
+        debug_more("%x ", addressed_out[i]);
+      debug_more("\n");
     }
-    for (i=0; i< out_size; i++)
-      printf("%x ", out[i]);
-    printf("\n");
     tpm_free(out);
+    tpm_free(addressed_out);
 
   } // loop
 
   tpm_emulator_shutdown();
 
-  close(tpm_tx_fh);
-  close(tpm_rx_fh);
+  close(vtpm_tx_fh);
+#ifndef VTPM_MUTLI_VM
+  close(vtpm_rx_fh);
+  free (vtpm_rx_file);
+#endif
 
 }