//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processCloseDevice(
    Connection  *connection
) {
	do
	{
		// there is no payload to read

		// Device required
		MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
		if (NULL == device)
		{
			LOG_E("processCloseDevice(): no device");
			writeResult(connection, MC_DRV_RSP_DEVICE_NOT_OPENED);
			break;
		}

		// No command data will be read
		// Unregister device object with connection
		device->close(connection);

		// there is no payload
		writeResult(connection, MC_DRV_RSP_OK);

	} while (false);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processOpenDevice(Connection *connection)
{
    MC_DRV_CMD_OPEN_DEVICE_struct cmdOpenDevice;
    RECV_PAYLOAD_FROM_CLIENT(connection, &cmdOpenDevice);

    // Check if device has been registered to the connection
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
    if (NULL != device) {
        LOG_E("processOpenDevice(): device already set");
        writeResult(connection, MC_DRV_ERR_DEVICE_ALREADY_OPEN);
        return;
    }

    LOG_I(" Opening deviceId %d ", cmdOpenDevice.deviceId);

    // Get device for device ID
    device = getDevice(cmdOpenDevice.deviceId);

    // Check if a device for the given name has been found
    if (device == NULL) {
        LOG_E("invalid deviceId");
        writeResult(connection, MC_DRV_ERR_UNKNOWN_DEVICE);
        return;
    }

    // Register device object with connection
    device->open(connection);

    // Return result code to client lib (no payload)
    writeResult(connection, MC_DRV_OK);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processGetMobiCoreVersion(
    Connection  *connection
)
{
    // there is no payload to read

    // Device required
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
    CHECK_DEVICE(device, connection);

    // Get <t-base version info from secure world.
    mcDrvRspGetMobiCoreVersion_t rspGetMobiCoreVersion;

    mcResult_t mcResult = device->getMobiCoreVersion(&rspGetMobiCoreVersion.payload);

    if (mcResult != MC_DRV_OK) {
        LOG_V("MC GET_MOBICORE_VERSION returned code %d", mcResult);
        writeResult(connection, mcResult);
        return;
    }

    rspGetMobiCoreVersion.header.responseId = MC_DRV_OK;
    connection->writeData(
        &rspGetMobiCoreVersion,
        sizeof(rspGetMobiCoreVersion));
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processCloseSession(
    Connection  *connection
) {
	do
	{
		// Read entire command data
		mcDrvCmdCloseSessionPayload_t  cmdCloseSessionPayload;
		uint32_t rlen = connection->readData(
							&cmdCloseSessionPayload,
							sizeof(cmdCloseSessionPayload));
		if (rlen != sizeof(cmdCloseSessionPayload))
		{
			LOG_E("processCloseSession(): CloseSessionPayload length error: %d",rlen);
			writeResult(connection, MC_DRV_RSP_PAYLOAD_LENGTH_ERROR);
			break;
		}

		// Device required
		MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
		if (NULL == device)
		{
			LOG_E("processCloseSession(): device is NULL");
			writeResult(connection, MC_DRV_RSP_DEVICE_NOT_OPENED);
			break;
		}

		device->closeSession(
					connection,
					cmdCloseSessionPayload.sessionId);

		// there is no payload
		writeResult(connection, MC_DRV_RSP_OK);

	} while (false);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processNotify(Connection  *connection)
{
    // Read entire command data
    MC_DRV_CMD_NOTIFY_struct  cmd;
    //RECV_PAYLOAD_FROM_CLIENT(connection, &cmd);
    void *payload = (void *)((uintptr_t)&cmd + sizeof(mcDrvCommandHeader_t));
    uint32_t payload_len = sizeof(cmd) - sizeof(mcDrvCommandHeader_t);
    uint32_t rlen = connection->readData(payload, payload_len);
    if ((int) rlen < 0) {
        LOG_E("reading from Client failed");
        /* it is questionable, if writing to broken socket has any effect here. */
        // NOTE: notify fails silently
        //writeResult(connection, MC_DRV_RSP_SOCKET_ERROR);
        return;
    }
    if (rlen != payload_len) {
        LOG_E("wrong buffer length %i received from Client", rlen);
        // NOTE: notify fails silently
        //writeResult(connection, MC_DRV_RSP_PAYLOAD_LENGTH_ERROR);
        return;
    }

    // Device required
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
    if (NULL == device) {
        LOG_V("%s: no device associated with connection", __FUNCTION__);
        // NOTE: notify fails silently
        // writeResult(connection,MC_DRV_RSP_DEVICE_NOT_OPENED);
        return;
    }

    device->notify(connection, cmd.sessionId);
    // NOTE: for notifications there is no response at all
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processGetMobiCoreVersion(
    Connection  *connection
) {
	// Device required
	MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
	if (NULL == device) {
		writeResult(connection, MC_DRV_RSP_DEVICE_NOT_OPENED);
		return;
	}

	// Get MobiCore version info from secure world.
	mcDrvRspGetMobiCoreVersion_t rspGetMobiCoreVersion;

	device->getMobiCoreVersion(
		&rspGetMobiCoreVersion.payload);

	uint32_t mcResult = rspGetMobiCoreVersion.payload.mcResult;
	if (MC_MCP_RET_OK != mcResult) {
		LOG_E("processGetMobiCoreVersion(): rspGetMobiCoreVersion mcResult %d", mcResult);
		writeResult(connection, MC_DRV_RSP_FAILED);
		return;
	}

	rspGetMobiCoreVersion.header.responseId = MC_DRV_RSP_OK;
	connection->writeData(
		&rspGetMobiCoreVersion,
		sizeof(rspGetMobiCoreVersion));
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processNqConnect(
    Connection  *connection
) {
	do
	{
		// Set up the channel for sending SWd notifications to the client
		// MC_DRV_CMD_NQ_CONNECT is only allowed on new connections not
		// associated with a device. If a device is registered to the
		// connection NQ_CONNECT is not allowed.

		// Read entire command data
		mcDrvCmdNqConnectPayload_t  cmdNqConnectPayload;
		size_t rlen = connection->readData(
										&(cmdNqConnectPayload),
										sizeof(cmdNqConnectPayload));
		if (rlen != sizeof(cmdNqConnectPayload))
		{
			LOG_E("processNqConnect(): NqConnect length error: %d",rlen);
			writeResult(connection,MC_DRV_RSP_PAYLOAD_LENGTH_ERROR);
			break;
		}

		// device must be empty
		MobiCoreDevice *device = (MobiCoreDevice *)(connection->connectionData);
		if (NULL != device)
		{
			LOG_E("processNqConnect(): device already set\n");
			writeResult(connection,MC_DRV_RSP_COMMAND_NOT_ALLOWED);
			break;
		}

		// Remove the connection from the list of known client connections
		for (int i = 0; i < MAX_SERVERS; i++) {
			servers[i]->detachConnection(connection);
		}

		device = getDevice(cmdNqConnectPayload.deviceId);
		if (NULL == device)
		{
			//TODO xgal: how about ...NO_SUCH_DEVICE
			LOG_E("processNqConnect(): no device found\n");
			writeResult(connection, MC_DRV_RSP_FAILED);
			break;
		}

		TrustletSession* ts = device->registerTrustletConnection(
								connection,
								&cmdNqConnectPayload);
		if (!ts) {
			LOG_E("processNqConnect(): registerTrustletConnection() failed!");
			writeResult(connection, MC_DRV_RSP_FAILED);
			break;
		}

		writeResult(connection, MC_DRV_RSP_OK);
		ts->processQueuedNotifications();
		

	} while (false);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processOpenDevice(
    Connection	*connection
) {
	do
	{
		// Read entire command data
		mcDrvCmdOpenDevicePayload_t cmdOpenDevicePayload;
		uint32_t rlen = connection->readData(
							&(cmdOpenDevicePayload),
							sizeof(cmdOpenDevicePayload));
		if (rlen != sizeof(cmdOpenDevicePayload))
		{
			LOG_E("processOpenDevice(): OpenSession length error: %d", rlen);
			writeResult(connection, MC_DRV_RSP_PAYLOAD_LENGTH_ERROR);
			break;
		}

		// Check if device has been registered to the connection
		MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
		if (NULL != device)
		{
			LOG_E("processOpenDevice(): device already set");
			writeResult(connection, MC_DRV_RSP_DEVICE_ALREADY_OPENED);
			break;
		}

		LOG_I("processOpenDevice(): deviceId is %d",
				cmdOpenDevicePayload.deviceId);

		// Get device for device ID
		device = getDevice(cmdOpenDevicePayload.deviceId);

		// Check if a device for the given name has been found
		if (NULL == device)
		{
			LOG_E("invalid deviceId");
			writeResult(connection, MC_DRV_INVALID_DEVICE_NAME);
			break;
		}

		// Register device object with connection
		if (false == device->open(connection))
		{
			LOG_E("processOpenDevice(): device->open() failed");
			writeResult(connection, MC_DRV_RSP_FAILED);
			break;
		}

		// Return result code to client lib (no payload)
		writeResult(connection, MC_DRV_RSP_OK);

	} while (false);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::dropConnection(
    Connection *connection
) {
	// Check if a Device has already been registered with the connection
	MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);

	if (device != NULL) {
		LOG_I("dropConnection(): closing still open device.");
		// A connection has been found and has to be closed
		device->close(connection);
	}
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processCloseSession(Connection *connection)
{
    MC_DRV_CMD_CLOSE_SESSION_struct cmdCloseSession;
    RECV_PAYLOAD_FROM_CLIENT(connection, &cmdCloseSession)

    // Device required
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
    CHECK_DEVICE(device, connection);

    mcResult_t ret = device->closeSession(connection, cmdCloseSession.sessionId);

    // there is no payload
    writeResult(connection, ret);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processUnmapBulkBuf(
    Connection  *connection
) {
	do
	{
		// Read entire command data
		mcDrvCmdUnmapBulkMemPayload_t cmdUnmapBulkMemPayload;
		uint32_t rlen = connection->readData(
							&cmdUnmapBulkMemPayload,
							sizeof(cmdUnmapBulkMemPayload));
		if (rlen != sizeof(cmdUnmapBulkMemPayload))
		{
			LOG_E("processMapBulkBuf(): UnmapBulkMem length error: %d", rlen);
			writeResult(connection, MC_DRV_RSP_PAYLOAD_LENGTH_ERROR);
			break;
		}

		// Device required
		MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
		if (NULL == device)
		{
			writeResult(connection, MC_DRV_RSP_DEVICE_NOT_OPENED);
			break;
		}

		// Unmap bulk memory from secure world
		mcDrvRspUnmapBulkMem_t rspUnmpaBulk;

		device->unmapBulk(
					connection,
					&cmdUnmapBulkMemPayload,
					&(rspUnmpaBulk.payload));

		uint32_t mcResult = rspUnmpaBulk.payload.mcResult;
		if (MC_MCP_RET_OK != mcResult)
		{
			LOG_E("processUnmapBulkBuf(): rspUnmpaBulk mcResult %d", mcResult);
			writeResult(connection, MC_DRV_RSP_FAILED);
			break;
		}

		rspUnmpaBulk.header.responseId = MC_DRV_RSP_OK;
		connection->writeData(
						&rspUnmpaBulk,
						sizeof(rspUnmpaBulk));

	} while (false);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processCloseDevice(
    Connection  *connection
)
{
    // there is no payload to read

    // Device required
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
    CHECK_DEVICE(device, connection);

    // No command data will be read
    // Unregister device object with connection
    device->close(connection);

    // there is no payload
    writeResult(connection, MC_DRV_OK);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::dropConnection(
    Connection *connection
)
{
    // Check if a Device has already been registered with the connection
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);

    if (device != NULL) {
        // A connection has been found and has to be closed
        LOG_I("dropConnection(): closing still open device.");

        // Make sure nobody else writes the MCP, e.g. netlink/socket server, cleanup of TAs
        device->mutex_mcp.lock();
        device->close(connection);
        device->mutex_mcp.unlock();
    }
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processNqConnect(Connection *connection)
{
    // Set up the channel for sending SWd notifications to the client
    // MC_DRV_CMD_NQ_CONNECT is only allowed on new connections not
    // associated with a device. If a device is registered to the
    // connection NQ_CONNECT is not allowed.

    // Read entire command data
    MC_DRV_CMD_NQ_CONNECT_struct cmd;
    RECV_PAYLOAD_FROM_CLIENT(connection, &cmd);

    // device must be empty since this is a new connection
    MobiCoreDevice *device = (MobiCoreDevice *)(connection->connectionData);
    if (device != NULL) {
        LOG_E("device already set\n");
        writeResult(connection, MC_DRV_ERR_NQ_FAILED);
        return;
    }

    // Remove the connection from the list of known client connections
    for (int i = 0; i < MAX_SERVERS; i++) {
        servers[i]->detachConnection(connection);
    }

    device = getDevice(cmd.deviceId);
    // Check if a device for the given name has been found
    if (NULL == device) {
        LOG_E("invalid deviceId");
        writeResult(connection, MC_DRV_ERR_UNKNOWN_DEVICE);
        return;
    }

    TrustletSession *ts = device->registerTrustletConnection(
                              connection,
                              &cmd);
    if (!ts) {
        LOG_E("registerTrustletConnection() failed!");
        writeResult(connection, MC_DRV_ERR_UNKNOWN);
        return;
    }

    writeResult(connection, MC_DRV_OK);
    ts->processQueuedNotifications();
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processMapBulkBuf(Connection *connection)
{
    MC_DRV_CMD_MAP_BULK_BUF_struct cmd;

    RECV_PAYLOAD_FROM_CLIENT(connection, &cmd);

    // Device required
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
    CHECK_DEVICE(device, connection);

    if (!device->lockWsmL2(cmd.handle)) {
        LOG_E("Couldn't lock the buffer!");
        writeResult(connection, MC_DRV_ERR_DAEMON_WSM_HANDLE_NOT_FOUND);
        return;
    }

    uint32_t secureVirtualAdr = (uint32_t)NULL;
    uint64_t pAddrL2 = device->findWsmL2(cmd.handle, connection->socketDescriptor);

    if (pAddrL2 == 0) {
        LOG_E("Failed to resolve WSM with handle %u", cmd.handle);
        writeResult(connection, MC_DRV_ERR_DAEMON_WSM_HANDLE_NOT_FOUND);
        return;
    }

    // Map bulk memory to secure world
    mcResult_t mcResult = device->mapBulk(connection, cmd.sessionId, cmd.handle, pAddrL2,
                                          cmd.offsetPayload, cmd.lenBulkMem, &secureVirtualAdr);

    if (mcResult != MC_DRV_OK) {
        writeResult(connection, mcResult);
        return;
    }

    mcDrvRspMapBulkMem_t rsp;
    rsp.header.responseId = MC_DRV_OK;
    rsp.payload.sessionId = cmd.sessionId;
    rsp.payload.secureVirtualAdr = secureVirtualAdr;
    connection->writeData(&rsp, sizeof(mcDrvRspMapBulkMem_t));
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processNotify(
    Connection  *connection
) {
	do
	{
		// Read entire command data
		mcDrvCmdNotifyPayload_t  cmdNotifyPayload;
		uint32_t rlen = connection->readData(
							&cmdNotifyPayload,
							sizeof(cmdNotifyPayload));
		if (sizeof(cmdNotifyPayload) != rlen)
		{
			LOG_E("processNotify(): NotifyPayload length error: %d", rlen);
			// NOTE: notify fails silently
			// writeResult(connection,MC_DRV_RSP_PAYLOAD_LENGTH_ERROR);
			break;
		}

		// Device required
		MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
		if (NULL == device)
		{
			LOG_E("processNotify(): device is NULL");
			// NOTE: notify fails silently
			// writeResult(connection,MC_DRV_RSP_DEVICE_NOT_OPENED);
			break;
		}

		// REV axh: we cannot trust the clientLib to give us a valid
		//          sessionId here. Thus we have to check that it belongs to
		//          the clientLib's process.

		device->notify(cmdNotifyPayload.sessionId);
		// NOTE: for notifications there is no response at all
	} while(0);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processUnmapBulkBuf(Connection *connection)
{
    MC_DRV_CMD_UNMAP_BULK_BUF_struct cmd;
    RECV_PAYLOAD_FROM_CLIENT(connection, &cmd)

    // Device required
    MobiCoreDevice *device = (MobiCoreDevice *) (connection->connectionData);
    CHECK_DEVICE(device, connection);

    // Unmap bulk memory from secure world
    uint32_t mcResult = device->unmapBulk(connection, cmd.sessionId, cmd.handle,
                                          cmd.secureVirtualAdr, cmd.lenBulkMem);

    if (mcResult != MC_DRV_OK) {
        LOG_V("MCP UNMAP returned code %d", mcResult);
        writeResult(connection, mcResult);
        return;
    }

    // TODO-2012-09-06-haenellu: Think about not ignoring the error case.
    device->unlockWsmL2(cmd.handle);

    writeResult(connection, MC_DRV_OK);
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processOpenTrustlet(Connection *connection)
{
    MC_DRV_CMD_OPEN_TRUSTLET_struct cmdOpenTrustlet;
    RECV_PAYLOAD_FROM_CLIENT(connection, &cmdOpenTrustlet);

    // Device required
    MobiCoreDevice  *device = (MobiCoreDevice *) (connection->connectionData);
    CHECK_DEVICE(device, connection);

    uint32_t total_len, rlen, len = cmdOpenTrustlet.trustlet_len;
    uint8_t *payload = (uint8_t *)malloc(len);
    uint8_t *p = payload;
    if (payload == NULL) {
        LOG_E("failed to allocate payload buffer");
        writeResult(connection, MC_DRV_ERR_DAEMON_SOCKET);
        return;
    }
    total_len = 0;
    while (total_len < len) {
        rlen = connection->readData(p, len - total_len);
        if ((int32_t)rlen < 0) {
            LOG_E("reading from Client failed");
            /* it is questionable, if writing to broken socket has any effect here. */
            writeResult(connection, MC_DRV_ERR_DAEMON_SOCKET);
            free(payload);
            return;
        }
        total_len += rlen;
        p += rlen;
    }

    // Get service blob from registry
    regObject_t *regObj = mcRegistryMemGetServiceBlob(cmdOpenTrustlet.spid, (uint8_t *)payload, len);

    // Free the payload object no matter what
    free(payload);
    if (regObj == NULL) {
        writeResult(connection, MC_DRV_ERR_TRUSTLET_NOT_FOUND);
        return;
    }

    if (regObj->len == 0) {
        free(regObj);
        writeResult(connection, MC_DRV_ERR_TRUSTLET_NOT_FOUND);
        return;
    }
    LOG_I(" Sharing Service loaded at %p with Secure World", (addr_t)(regObj->value));

    CWsm_ptr pWsm = device->registerWsmL2((addr_t)(regObj->value), regObj->len, 0);
    if (pWsm == NULL) {
        free(regObj);
        LOG_E("allocating WSM for Trustlet failed");
        writeResult(connection, MC_DRV_ERR_DAEMON_KMOD_ERROR);
        return;
    }
    // Initialize information data of open session command
    loadDataOpenSession_t loadDataOpenSession;
    loadDataOpenSession.baseAddr = pWsm->physAddr;
    loadDataOpenSession.offs = ((uintptr_t) regObj->value) & 0xFFF;
    loadDataOpenSession.len = regObj->len;
    loadDataOpenSession.tlHeader = (mclfHeader_ptr) (regObj->value + regObj->tlStartOffset);

    mcDrvRspOpenSession_t rspOpenSession;
    mcResult_t ret = device->openSession(
                         connection,
                         &loadDataOpenSession,
                         cmdOpenTrustlet.handle,
                         cmdOpenTrustlet.len,
                         cmdOpenTrustlet.tci,
                         &rspOpenSession.payload);

    // Unregister physical memory from kernel module.
    LOG_I(" Service buffer was copied to Secure world and processed. Stop sharing of buffer.");

    // This will also destroy the WSM object.
    if (!device->unregisterWsmL2(pWsm)) {
        free(regObj);
        // TODO-2012-07-02-haenellu: Can this ever happen? And if so, we should assert(), also TL might still be running.
        writeResult(connection, MC_DRV_ERR_DAEMON_KMOD_ERROR);
        return;
    }

    // Free memory occupied by Trustlet data
    free(regObj);

    if (ret != MC_DRV_OK) {
        LOG_E("Service could not be loaded.");
        writeResult(connection, ret);
    } else {
        rspOpenSession.header.responseId = ret;
        connection->writeData(
            &rspOpenSession,
            sizeof(rspOpenSession));
    }
}
//------------------------------------------------------------------------------
mcResult_t MobiCoreDriverDaemon::processLoadCheck(mcSpid_t spid, void *blob, uint32_t size)
{

    // Device required
    MobiCoreDevice  *device = getDevice(MC_DEVICE_ID_DEFAULT);

    if (device == NULL) {
        LOG_E(" No device found");
        return MC_DRV_ERR_DAEMON_DEVICE_NOT_OPEN;
    }

    // Get service blob from registry
    regObject_t *regObj = mcRegistryMemGetServiceBlob(spid, blob, size);
    if (NULL == regObj) {
        LOG_E(" mcRegistryMemGetServiceBlob failed");
        return MC_DRV_ERR_TRUSTLET_NOT_FOUND;
    }
    if (regObj->len == 0) {
        free(regObj);
        LOG_E("mcRegistryMemGetServiceBlob returned registry object with length equal to zero");
        return MC_DRV_ERR_TRUSTLET_NOT_FOUND;
    }
    LOG_I(" Sharing Service loaded at %p with Secure World", (addr_t)(regObj->value));

    CWsm_ptr pWsm = device->registerWsmL2((addr_t)(regObj->value), regObj->len, 0);
    if (pWsm == NULL) {
        // Free memory occupied by Trustlet data
        free(regObj);
        LOG_E("allocating WSM for Trustlet failed");
        return MC_DRV_ERR_DAEMON_KMOD_ERROR;
    }
    // Initialize information data of open session command
    loadDataOpenSession_t loadDataOpenSession;
    loadDataOpenSession.baseAddr = pWsm->physAddr;
    loadDataOpenSession.offs = ((uintptr_t) regObj->value) & 0xFFF;
    loadDataOpenSession.len = regObj->len;
    loadDataOpenSession.tlHeader = (mclfHeader_ptr) (regObj->value + regObj->tlStartOffset);

    mcDrvRspOpenSession_t rspOpenSession;
    mcResult_t ret = device->checkLoad(
                         &loadDataOpenSession,
                         &rspOpenSession.payload);

    // Unregister physical memory from kernel module.
    LOG_I(" Service buffer was copied to Secure world and processed. Stop sharing of buffer.");

    // This will also destroy the WSM object.
    if (!device->unregisterWsmL2(pWsm)) {
        // Free memory occupied by Trustlet data
        free(regObj);
        LOG_E("deallocating WSM for Trustlet failed");
        return MC_DRV_ERR_DAEMON_KMOD_ERROR;
    }

    // Free memory occupied by Trustlet data
    free(regObj);

    if (ret != MC_DRV_OK) {
        LOG_E("TA could not be loaded.");
        return ret;
    } else {
        return MC_DRV_OK;
    }
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processOpenSession(Connection *connection, bool isGpUuid)
{
    MC_DRV_CMD_OPEN_SESSION_struct cmdOpenSession;
    RECV_PAYLOAD_FROM_CLIENT(connection, &cmdOpenSession);

    // Device required
    MobiCoreDevice  *device = (MobiCoreDevice *) (connection->connectionData);
    CHECK_DEVICE(device, connection);

    // Get service blob from registry
    regObject_t *regObj = mcRegistryGetServiceBlob(&cmdOpenSession.uuid, isGpUuid);
    if (NULL == regObj) {
        writeResult(connection, MC_DRV_ERR_TRUSTLET_NOT_FOUND);
        return;
    }
    if (regObj->len == 0) {
        free(regObj);
        writeResult(connection, MC_DRV_ERR_TRUSTLET_NOT_FOUND);
        return;
    }
    LOG_I(" Sharing Service loaded at %p with Secure World", (addr_t)(regObj->value));

    CWsm_ptr pWsm = device->registerWsmL2((addr_t)(regObj->value), regObj->len, 0);
    if (pWsm == NULL) {
        // Free memory occupied by Trustlet data
        free(regObj);
        LOG_E("allocating WSM for Trustlet failed");
        writeResult(connection, MC_DRV_ERR_DAEMON_KMOD_ERROR);
        return;
    }
    // Initialize information data of open session command
    loadDataOpenSession_t loadDataOpenSession;
    loadDataOpenSession.baseAddr = pWsm->physAddr;
    loadDataOpenSession.offs = ((uintptr_t) regObj->value) & 0xFFF;
    loadDataOpenSession.len = regObj->len;
    loadDataOpenSession.tlHeader = (mclfHeader_ptr) (regObj->value + regObj->tlStartOffset);

    mcDrvRspOpenSession_t rspOpenSession;
    mcResult_t ret = device->openSession(
                         connection,
                         &loadDataOpenSession,
                         cmdOpenSession.handle,
                         cmdOpenSession.len,
                         cmdOpenSession.tci,
                         &rspOpenSession.payload);

    // Unregister physical memory from kernel module.
    LOG_I(" Service buffer was copied to Secure world and processed. Stop sharing of buffer.");

    // This will also destroy the WSM object.
    if (!device->unregisterWsmL2(pWsm)) {
        // TODO-2012-07-02-haenellu: Can this ever happen? And if so, we should assert(), also TL might still be running.
        free(regObj);
        writeResult(connection, MC_DRV_ERR_DAEMON_KMOD_ERROR);
        return;
    }

    // Free memory occupied by Trustlet data
    free(regObj);

    if (ret != MC_DRV_OK) {
        LOG_E("Service could not be loaded.");
        writeResult(connection, ret);
    } else {
        rspOpenSession.header.responseId = ret;
        connection->writeData(
            &rspOpenSession,
            sizeof(rspOpenSession));
    }
}
//------------------------------------------------------------------------------
void MobiCoreDriverDaemon::processOpenSession(
    Connection  *connection
) {
	do
	{
		// Read entire command data
		mcDrvCmdOpenSessionPayload_t  cmdOpenSessionPayload;
		uint32_t rlen = connection->readData(
							&cmdOpenSessionPayload,
							sizeof(cmdOpenSessionPayload));
		if (rlen != sizeof(cmdOpenSessionPayload))
		{
			LOG_E("processOpenSession(): OpenSession length error: %d", rlen);
			writeResult(connection, MC_DRV_RSP_PAYLOAD_LENGTH_ERROR);
			break;
		}

		// Device required
		MobiCoreDevice  *device = (MobiCoreDevice *) (connection->connectionData);
		if (NULL == device)
		{
			writeResult(connection, MC_DRV_RSP_DEVICE_NOT_OPENED);
			break;
		}

		// Get service blob from registry
		regObject_t *regObj = mcRegistryGetServiceBlob(
				(mcUuid_t*) &(cmdOpenSessionPayload.uuid));

		// Call preinstalled variant of method
		mcDrvRspOpenSession_t rspOpenSession;
		if (NULL == regObj)
		{
			writeResult(connection, MC_DRV_RSP_TRUSTLET_NOT_FOUND);
			break;
		}
		if (0 == regObj->len)
		{
			free(regObj);
			writeResult(connection, MC_DRV_RSP_TRUSTLET_NOT_FOUND);
			break;
		}
		else
		{
			// Trustlet retrieved from registry

			LOG_I("registering L2 in kmod, p=%p, len=%i",
					regObj->value,
					regObj->len);

			// Prepare the interface structure for memory registration, then
			// register virtual memory in kernel module, create L2 table
			// TODO xgal: refactor naming of datatypes and WSM handling
			CWsm_ptr pWsm = device->registerWsmL2(
				(addr_t)(regObj->value),
				regObj->len,
				0);
			if (NULL == pWsm)
			{
				LOG_E("allocating WSM for Trustlet failed");
				writeResult(connection, MC_DRV_RSP_FAILED);
				break;
			}
			// Initialize information data of open session command
			loadDataOpenSession_t loadDataOpenSession;
			loadDataOpenSession.baseAddr = pWsm->physAddr;
			loadDataOpenSession.offs = ((uint32_t) regObj->value) & 0xFFF;
			loadDataOpenSession.len = regObj->len;
			loadDataOpenSession.tlHeader = (mclfHeader_ptr) regObj->value;
			
			device->openSession(
						connection,
						&loadDataOpenSession,
						&cmdOpenSessionPayload,
						&(rspOpenSession.payload));

			// Unregister physical memory from kernel module.
			// This will also destroy the WSM object.
			if(!device->unregisterWsmL2(pWsm)) {
				writeResult(connection, MC_DRV_RSP_FAILED);
				break;
			}

			// Free memory occupied by Trustlet data
			free(regObj);
		}

		uint32_t mcResult = rspOpenSession.payload.mcResult;
		if (MC_MCP_RET_OK != mcResult)
		{
			LOG_E("rspOpenSession mcResult %d", mcResult);
			writeResult(connection, MC_DRV_RSP_FAILED);
			break;
		}

		rspOpenSession.header.responseId = MC_DRV_RSP_OK;
		connection->writeData(
						&rspOpenSession,
						sizeof(rspOpenSession));

	} while (false);
}