/*
 * Copyright (c) 2015 Texas Instruments Incorporated - http://www.ti.com
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 *  ======== Control.c ========
 */

/* this define must precede inclusion of any xdc header file */
#define Registry_CURDESC Control__Desc
#define MODULE_NAME "Control"

/* xdctools header files */
#include <xdc/std.h>
#include <xdc/runtime/Diags.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/Log.h>
#include <xdc/runtime/Registry.h>
#include <xdc/runtime/System.h>

/* package header files */
#include <ti/ipc/Ipc.h>
#include <ti/ipc/MessageQ.h>
#include <ti/ipc/ipcmgr/IpcMgr.h>
#include <ti/sdo/ipc/interfaces/INetworkTransport.h>
#include <ti/sdo/ipc/interfaces/ITransport.h>

#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Task.h>

/* local header files */
#include "Compute.h"
#include "Control.h"
#include "PEB.h"
#include "../shared/Control_shared.h"
#include "../shared/ResMgr_shared.h"
//#if 0
#include <transport/rtos/TransportQMSS.h>

#define TRANSPORT_QMSS_ID       1
TransportQMSS_Handle    Control_transQMSS;
//#endif // 0

/* private data */
Registry_Desc       Registry_CURDESC;
Semaphore_Handle    Control_sem;
MessageQ_Handle     Control_queue;      /* inbound messages */
MessageQ_QueueId    Control_rmQue;      /* resoure manager queue */



/*
 *  ======== Control_taskFxn ========
 */
Void Control_taskFxn(UArg arg0, UArg arg1)
{
    Int         status = 0;
    Error_Block eb;
    Char        cbuf[48];
    Bool        ready;

    Registry_Result     result;
    Semaphore_Params    semParams;
    MessageQ_Params     msgqParams;
    MessageQ_Msg        mqMsg, appMsg;
    MessageQ_QueueId    qid;
    Control_Msg        *msg;
    ResMgr_Msg         *rmMsg;
    UInt16              hostProcId;
//#if 0
    ITransport_Handle           transport;
    INetworkTransport_Handle    transNetwork;
    TransportQMSS_Params        transQmssParams;
//#endif //0
    Error_init(&eb);

    /* register with xdc.runtime to get a diags mask */
    result = Registry_addModule(&Registry_CURDESC, MODULE_NAME);

    if ((result != Registry_SUCCESS) && (result != Registry_ALREADY_ADDED)) {
        status = -1;
        goto leave;
    }

    /* enable some trace */
    Diags_setMask(MODULE_NAME"+F");
    Log_print0(Diags_INFO, "Control_taskFxn: -->");

    /* initialize the semaphore */
    Semaphore_Params_init(&semParams);
    semParams.mode = Semaphore_Mode_BINARY;

    Control_sem = Semaphore_create(0, &semParams, &eb);

    if (Error_check(&eb)) {
        status = -2;
        goto leave;
    }

    /* setup IPC */
    Log_print0(Diags_INFO, "Control_taskFxn: initializing IPC");

    /* setup TransportRpmsg for host communication */
    IpcMgr_ipcStartup();

    /* setup IPC for slave to slave communication */
    status = Ipc_start();

    if (status < 0) {
        Log_error0("Control_TaskFxn: Ipc_start failed");
        goto leave;
    }

    Log_print0(Diags_INFO, "Control_taskFxn: IPC startup complete");

    /* compute the message queue name */
    System_sprintf(cbuf, Control_QueNameFmt, MultiProc_self());

    /* create message queue (inbound messages) */
    MessageQ_Params_init(&msgqParams);
    Control_queue = MessageQ_create(cbuf, &msgqParams);

    if (Control_queue == NULL) {
        Log_error0("Control_TaskFxn: message queue create failed");
        goto leave;
    }

    /* inform other modules that IPC is ready */
    Compute_ipcIsReady();
    PEB_ipcIsReady();

    /* busy wait until other modules are ready */
    do {
        ready = TRUE;

        if (!Compute_isReady()) {
            ready = FALSE;
        }

        if (!PEB_isReady()) {
            ready = FALSE;
        }

        if (!ready) {
            Task_sleep(10);
        }

    } while (!ready);

    Log_print0(Diags_INFO, "Control_taskFxn: peer modules are ready");

    /* wait for application start message */
    Log_print0(Diags_INFO, "Control_taskFxn: waiting for start message");
    status = MessageQ_get(Control_queue, &appMsg, MessageQ_FOREVER);

    if (status < 0) {
        Log_error1("Control_taskFxn: message get error=%d", (IArg)status);
        goto leave;
    }

    /* process the message */
    msg = (Control_Msg *)appMsg;

    if (msg->command != Control_Cmd_INITIALIZE) {
        Log_print1(Diags_INFO, "Control_taskFxn: Warning: "
                "discarding message, cmd=%d", (IArg)msg->command);
        status = -1;
        goto leave;
    }

    /* open the resource manager message queue on the host */
    hostProcId = MultiProc_getId("HOST");
    Control_rmQue = MessageQ_openQueueId(ResMgr_QueIndex, hostProcId);

    /* request resource from the resource manager */
    mqMsg = MessageQ_alloc(0, sizeof(ResMgr_Msg));

    if (mqMsg == NULL) {
        status = -1;
        Log_error0("Control_taskFxn: message alloc failed");
        goto leave;
    }

    rmMsg = (ResMgr_Msg *)mqMsg;
    rmMsg->cmd = ResMgr_Cmd_REQUEST;
    rmMsg->arg1 = MultiProc_self();
    rmMsg->arg2 = 8;

    /* set return address */
    MessageQ_setReplyQueue(Control_queue, mqMsg);

    /* send request to resource manager */
    status = MessageQ_put(Control_rmQue, mqMsg);

    if (status < 0) {
        Log_error1("message put error=%d", (IArg)status);
        status = -2;
        goto leave;
    }
    Log_print0(Diags_INFO, "Control_taskFxn: RM message sent");

    /* wait for reply message from resource manager */
    status = MessageQ_get(Control_queue, &mqMsg, MessageQ_FOREVER);

    if (status < 0) {
        Log_error1("Control_taskFxn: message get error=%d", (IArg)status);
        goto leave;
    }

    MessageQ_free(mqMsg);
//#if 0
    /* create the qmss transport instance */
    TransportQMSS_Params_init(&transQmssParams);

    Control_transQMSS = TransportQMSS_create(&transQmssParams, &eb);

    if (Error_check(&eb)) {
        Log_error0("transport qmss create failed");
        status = -3;
        goto leave;
    }

    /* register qmss transport with MessageQ */
    transNetwork = TransportQMSS_Handle_upCast(Control_transQMSS);
    transport = INetworkTransport_Handle_upCast(transNetwork);
    MessageQ_registerTransportId(TRANSPORT_QMSS_ID, transport);
    Log_print0(Diags_INFO, "TransportQMSS instance created");

    /* return application start message */
    qid = MessageQ_getReplyQueue(appMsg);

    status = MessageQ_put(qid, appMsg);

    if (status < 0) {
        Log_error1("Control_taskFxn: message put error=%d", (IArg)status);
        goto leave;
    }
//#endif // 0 Disable by Sarath, Thinking that unwanted code
    /* wait here */
    Log_print0(Diags_INFO, "Control_taskFxn: waiting for work");
    Semaphore_pend(Control_sem, BIOS_WAIT_FOREVER);


    /* finalize phase */

    /* delete the message queue */
    MessageQ_delete(&Control_queue);

leave:
    Log_print1(Diags_INFO, "Control_taskFxn: <-- %d", (IArg)status);
}
