//------------------------------------------------------------------------------
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::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));
    }
}
//------------------------------------------------------------------------------
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);
}