/*---------------------------------------------------------------------------*
  Project:  MPDS library
  File:     mpdssystem.c

  Copyright 2007 Nintendo. All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Log: mpdssystem.c,v $
  Revision 1.11  2007/11/29 03:17:55  seiki_masashi
  Added the const modifier.

  Revision 1.10  2007/10/24 13:45:24  seiki_masashi
  Changed so that OSThreadQueue is used during synchronization.

  Revision 1.9  2007/10/24 08:47:14  seiki_masashi
  Changed constant names.

  Revision 1.8  2007/10/22 10:27:04  seiki_masashi
  Renamed doubleMode to isDoubleMode

  Revision 1.7  2007/10/22 09:41:04  seiki_masashi
  Made the MPDSStep function into a synchronous function, and added the new function MPDSTryStep

  Revision 1.6  2007/10/18 01:36:13  seiki_masashi
  Fixed debug output

  Revision 1.5  2007/10/18 00:31:14  seiki_masashi
  Small fix

  Revision 1.4  2007/10/17 12:47:48  seiki_masashi
  Changed the constants for debug output

  Revision 1.3  2007/10/17 09:43:22  seiki_masashi
  Added debug output

  Revision 1.2  2007/10/11 00:16:13  seiki_masashi
  Small fix

  Revision 1.1  2007/10/10 08:37:07  seiki_masashi
  Added the MPDS library

  $NoKeywords: $
 *---------------------------------------------------------------------------*/

#include <revolution/revodefs.h>
REVOLUTION_LIB_VERSION( MPDS );

#include <revolution.h>
#include <revolution/net.h>
#include <revolution/mpds.h>

#include "mpdsprivate.h"


/*---------------------------------------------------------------------------*
    Constant Definitions
 *---------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*
    Internal Variable Definitions
 *---------------------------------------------------------------------------*/

static BOOL  MPDSi_registered     = FALSE;

u32 MPDSi_debugLevel = MPDS_DEBUG_LEVEL_DEFAULT;


/*---------------------------------------------------------------------------*
    Internal Function Prototype Definitions
 *---------------------------------------------------------------------------*/

static u8 CountPopulation(u32 x);
static void PortCallback( s32 type, MPPortCallbackInfo* info );
static inline u16 GetNextIndex(u32 index)
{
    return (u16)((index + 1) % MPDS_DATASET_NUM);
}

static inline u16 GetPrevIndex(u32 index)
{
    return (u16)((index + MPDS_DATASET_NUM - 1) % MPDS_DATASET_NUM);
}

static void Start( MPDSContext* context );
static void Stop( MPDSContext* context );
static void SignalDataSet( MPDSContext* context );
static s32 Step( MPDSContext* context, const void* sendData, MPDSDataSet* recvDataSet, BOOL isSync );
static void ReceiveData(MPDSContext *context, u32 aid, u8 *data);
static void SendDataSet(MPDSContext *context, BOOL delayed);
static u8* GetSharedDataAddress( const MPDSContext *context, u32 aidBits, u16 *receiveBuf, u32 aid);
static void SetDataCallback(s32 result, MPCallbackInfo *info);
static void ReceiveCallback_Parent(s32 type, MPPortCallbackInfo *info);

#if 0
#define MPDSiLOG_AIDBITS_BEGIN( context, index )  { u16 __prev_aidbits = (context)->ds[(index)].aidBits
#define MPDSiLOG_AIDBITS_END( context, index )  MPDSiLog( DETAIL_AIDBITS, "ds[%d].aidBits = %04x -> %04x", (index), __prev_aidbits, (context)->ds[(index)].aidBits ); }
#else
#define MPDSiLOG_AIDBITS_BEGIN( context, index )  ((void)0)
#define MPDSiLOG_AIDBITS_END( context, index )  ((void)0)
#endif



/*---------------------------------------------------------------------------*
  Name        : MPDSInit
  Description : Configure the initial settings for data sharing
  Arguments   : context - The data sharing context structure
                config  - Data sharing settings
  Returns     : s32     - Returns the result of the process. Returns a negative value if processing failed.
 *---------------------------------------------------------------------------*/
s32
MPDSInit( MPDSContext* context, const MPDSConfig* config )
{
    u32 aidBits;

    if ( MPDSi_registered == FALSE )
    {
        MPDSi_registered = TRUE;
        OSRegisterVersion( __MPDSVersion );
    }

    if ( context == NULL || config == NULL )
    {
        MPDSiError("Invalid argument");
        return MP_RESULT_ILLEGAL_PARAMETER;
    }

    (void)NETMemSet( context, 0, sizeof(MPDSContext) );
    (void)NETMemCpy( &context->config, config, sizeof(MPDSConfig) );

    aidBits = config->aidBits | 0x0001; // Parents are always included
    context->config.aidBits = aidBits;

    if ( config->port < MP_RAW_PORT_MAX
         || (config->aidBits & MP_AID_BITS_MASK) == 0
         || (config->aidBits & ~MP_AID_BITS_MASK) != 0 )
    {
        MPDSiError("Invalid config");
        return MP_RESULT_ILLEGAL_PARAMETER;
    }

    if ( !config->isParent )
    {
        MPDSiError("config.isParent have to be TRUE");
        return MP_RESULT_ILLEGAL_PARAMETER;
    }

    if ( !config->isAutoStart )
    {
        MPDSiError("config.isAutoStart have to be TRUE");
        return MP_RESULT_ILLEGAL_PARAMETER;
    }

    OSInitThreadQueue(&context->readThreadQueue);

    {
        u16 count = CountPopulation(aidBits);
        context->stationNumber = count;
        context->dataSetLength = (u16)(context->config.dataLength * count);

        if (context->dataSetLength > MPDS_DATA_SIZE)
        {
            // Total data amount exceeds limit
            context->config.aidBits = 0;
            MPDSiError("Total size of sharing data must be less than or equal to %d bytes.", MPDS_DATA_SIZE);
            return MP_RESULT_ILLEGAL_PARAMETER;
        }
        context->dataSetLength += 4;    // aidBits, receivedBits
    }

    context->state = MPDS_STATE_READY;

    return MP_RESULT_OK;
}

void Start( MPDSContext* context )
{
    OSWakeupThread(&context->readThreadQueue); // Just in case

    if ( context->config.isParent )
    {
        int     i;

        for (i = 0; i < MPDS_DATASET_NUM; i++)
        {
            MPDSiLOG_AIDBITS_BEGIN( context, i );
            context->ds[i].aidBits = MPHToMP16(0x0001);
            MPDSiLOG_AIDBITS_END( context, i );
        }

        // Set empty data for startup
        for (i = 0; i < ((context->config.isDoubleMode == TRUE) ? 2 : 1); i++)
        {
            context->writeIndex = GetNextIndex(context->writeIndex);
            context->seqNum[i] = 0xffff;
            context->sendIndex = GetNextIndex(context->sendIndex);
            SignalDataSet(context);
        }
    }

    context->state = MPDS_STATE_START;
}

void Stop( MPDSContext* context )
{
    context->state = MPDS_STATE_READY;

    OSWakeupThread(&context->readThreadQueue);
}

void SignalDataSet( MPDSContext* context )
{
    OSWakeupThread(&context->readThreadQueue);
    {
        MPPortCallback cb = context->config.mpdsCallback;
        if ( cb != NULL )
        {
            MPPortCallbackInfo* info = &context->tmpCallbackInfo;
            (void)NETMemSet( info, 0, sizeof(MPPortCallbackInfo) );
            info->type = MPDS_PORT_CB_TYPE_DATASET_RECEIVED;
            info->result = MP_RESULT_OK;
            info->port = context->config.port;
            info->userData = context;
            cb(info->type, info);
        }
    }
}

/*---------------------------------------------------------------------------*
  Name        : MPDSSetupPortConfig
  Description : Configure MP communications for data sharing
  Arguments   : context - The data sharing context structure
                mpconfig - MP communication settings
  Returns     : s32     - Returns the result of the processing. Returns a negative value if processing failed.
 *---------------------------------------------------------------------------*/
s32
MPDSSetupPortConfig( const MPDSContext* context, MPConfig* mpconfig )
{
    // Check the parameter
    if (context == NULL || mpconfig == NULL)
    {
        MPDSiError("Invalid parameter");
        return MP_RESULT_ILLEGAL_PARAMETER;
    }

    if ( context->config.isParent )
    {
        MPSetPortConfigEx( mpconfig, context->config.port, ReceiveCallback_Parent, context->config.priority, (void*)context );
    }
#ifdef MPDS_ENABLE_CHILD
    else
    {
        MPSetPortConfigEx( mpconfig, context->config.port, ReceiveCallback_Child, context->config.priority, (void*)context );
    }
#endif

    return MP_RESULT_OK;
}

#if 0
/*---------------------------------------------------------------------------*
  Name        : MPDSStart
  Description : Start MP communications for data sharing
  Arguments   : context - The data sharing context structure
  Returns     : s32     - Returns the result of the processing. Returns a negative value if processing failed.
 *---------------------------------------------------------------------------*/
s32
MPDSStart( MPDSContext* context )
{
    // Check the parameter
    if (context == NULL)
    {
        MPDSiError("Invalid parameter");
        return MP_RESULT_ILLEGAL_PARAMETER;
    }

    if ( context->config.isParent )
    {
        // Parent's start processing
        int  i;
        u32  mpReadyBits = MPGetConnectedAIDs();

        for (i = 0; i < MPDS_DATASET_NUM; i++)
        {
            context->ds[i].aidBits = MPHToMP16((u16)(context->config.aidBits & (mpReadyBits | 0x0001)));
        }

        // Sends empty data for start-up
        for (i = 0; i < ((context->config.isDoubleMode == TRUE) ? 2 : 1); i++)
        {
            int     res;

            MPDSiTrace( "send ds[%d] to aids %08x", i, (context->config.aidBits & mpReadyBits) );
            context->writeIndex = GetNextIndex(context->writeIndex);
            res =
                MPSendAsync( &context->ds[i], context->dataSetLength,
                             (u16)(context->config.aidBits & mpReadyBits), context->config.port,
                             NULL, SetDataCallback, (void*)context );
            if ( res != MP_RESULT_OK )
            {
                MPDSiWarning("MPSendAsync failed during data-sharing. result=%x", res);
                context->state = MPDS_STATE_ERROR;
                return res;
            }
        }
    }
#ifdef MPDS_ENABLE_CHILD
    else
    {
        // Child's start processing
        context->sendIndex = (u16)(MPDS_DATASET_NUM - 1);
    }
#endif

    return MP_RESULT_OK;         // Successful completion
}

/*---------------------------------------------------------------------------*
  Name        : MPDSStop
  Description : Stop data sharing
  Arguments   : context - The data sharing context structure
  Returns     : s32     - Returns the result of the processing. Returns a negative value if processing failed.
 *---------------------------------------------------------------------------*/
s32
MPDSStop( MPDSContext* context )
{
}
#endif

/*---------------------------------------------------------------------------*
  Name        : MPDSStep
  Description : Perform data sharing
  Arguments   : context - The data sharing context structure
                sendData - The data that the local host wants to share next
                recvDataSet - The data that was shared
  Returns     : s32     - Returns the result of the processing. Returns a negative value if processing failed.
 *---------------------------------------------------------------------------*/
s32
MPDSStep( MPDSContext* context, const void* sendData, MPDSDataSet* recvDataSet )
{
    return Step( context, sendData, recvDataSet, TRUE );
}

/*---------------------------------------------------------------------------*
  Name        : MPDSTryStep
  Description : Perform data sharing
  Arguments   : context - The data sharing context structure
                sendData - The data that the local host wants to share next
                recvDataSet - The data that was shared
  Returns     : s32     - Returns the result of the processing. Returns a negative value if processing failed.
 *---------------------------------------------------------------------------*/
s32
MPDSTryStep( MPDSContext* context, const void* sendData, MPDSDataSet* recvDataSet )
{
    return Step( context, sendData, recvDataSet, FALSE );
}

s32
Step( MPDSContext* context, const void* sendData, MPDSDataSet* recvDataSet, BOOL isSync )
{
    s32     result;
    u32     mpReadyBits;
    u32     state;

    // Check the parameter
    if ( context == NULL || sendData == NULL || recvDataSet == NULL )
    {
        MPDSiWarning("Invalid parameter");
        return MP_RESULT_ILLEGAL_PARAMETER;
    }

    // Checks the data sharing state
    state = context->state;
    if (state == MPDS_STATE_ERROR)
    {
        MPDSiWarning("An error occurred during data-sharing.");
        return MP_RESULT_ILLEGAL_STATE;
    }
    if (state != MPDS_STATE_START && state != MPDS_STATE_RETRY_SEND)
    {
        MPDSiWarning("It is not data-sharing mode now.");
        return MP_RESULT_ILLEGAL_STATE;
    }

    result = MP_RESULT_NO_DATA;

    if ( context->config.isParent )
    {
        // If a parent
        BOOL    sendFlag = FALSE;
        BOOL    delayed = FALSE;

        mpReadyBits = MPGetConnectedAIDs();

        if (state == MPDS_STATE_RETRY_SEND)
        {
            // Resends because the previous time was SEND_QUEUE_FULL
            int     res;
            int     oldWI;
            context->state = MPDS_STATE_START;

            MPDSiReport("send queue was full. do retry.");

            oldWI = GetPrevIndex(context->writeIndex);
            MPDSiTrace( "resend ds[%d] to aids %08x", oldWI, (context->config.aidBits & mpReadyBits) );
            res =
                MPSendAsync( &context->ds[oldWI], context->dataSetLength,
                             (u16)(context->config.aidBits & mpReadyBits), context->config.port,
                             NULL, SetDataCallback, (void*)context );
            if (res != MP_RESULT_OK)
            {
                MPDSiWarning("MPSendAsync failed during data-sharing. result=%x", res);
                context->state = MPDS_STATE_ERROR;
                return res;
            }
        }

        while ( context->state == MPDS_STATE_START )
        {
            // After the parent device has finished sending DataSet, it will be available for use, so decide the limit up to which sendIndex can read.
            if (context->readIndex != context->sendIndex)
            {
                // Read processing
                // Because readIndex is only manipulated here, there is no need to disable interrupts
                MPDSiTrace("read ds   : write: %d, read: %d, send: %d, seq#: %d",
                           context->writeIndex, context->readIndex, context->sendIndex,
                           context->seqNum[context->readIndex]);

                MPDSiLOG_AIDBITS_BEGIN( context, context->readIndex );
                context->ds[context->readIndex].aidBits |= MPHToMP16(0x0001);  // The lowest bit in the aidBits in the transmission data is a delay flag.
                MPDSiLOG_AIDBITS_END( context, context->readIndex );
                (void)NETMemCpy(recvDataSet, &context->ds[context->readIndex], sizeof(MPDSDataSet));
                context->currentSeqNum = context->seqNum[context->readIndex];
                context->readIndex = GetNextIndex(context->readIndex);

                sendFlag = TRUE;
                result = MP_RESULT_OK;
                if (context->config.isDoubleMode == FALSE && mpReadyBits != 0
                    && context->ds[context->writeIndex].aidBits == MPHToMP16(0x0001))
                {
                    // If the parent sets data, it can be sent immediately = only the parent has frame shifts.
                    delayed = TRUE;
                }
                else
                {
                    delayed = FALSE;
                }
            }

            if ( isSync && result == MP_RESULT_NO_DATA )
            {
                // If called in synchronous mode, and there was no received data, then sleep.
                OSSleepThread(&context->readThreadQueue);
            }
            else
            {
                break;
            }
        }
        if ( context->state != MPDS_STATE_START )
        {
            result = MP_RESULT_ILLEGAL_STATE;
        }

        // Sends the DataSet when the data for all the devices is ready in the send buffer
        SendDataSet(context, FALSE);

        if (sendFlag)
        {
            // The parent inserts into its own buffer instead of sending over wireless
            MPDSiTrace("send data : write: %d, read: %d, send: %d", context->writeIndex,
                                 context->readIndex, context->sendIndex);

            ReceiveData(context, 0, (u8 *)sendData);       // Remove const because chained rewrite past here is bothersome.

            if (context->config.isDoubleMode == FALSE)
            {
                // When and only when there is not a Step in each frame, DataSet needs to be prepared at this time for sending with GF.
                // 
                // Sends the DataSet when the data for all the devices is ready in the send buffer
                SendDataSet(context, delayed);
            }
        }
    }
#ifdef MPDS_ENABLE_CHILD
    else
    {
        // If a child
        BOOL    sendFlag = FALSE;

        if (state == MPDS_STATE_RETRY_SEND)
        {
            // Resends because the previous time was SEND_QUEUE_FULL
            sendFlag = TRUE;
            context->state = MPDS_STATE_START;
            MPDSiReport("send queue was full. do retry.");
        }
        else
        {
            while ( context->state == MPDS_STATE_START )
            {
                // After the child device has finished receiving DataSet, it will be available for use, so decide the limit up to which writeIndex can read.
                if (context->readIndex != context->writeIndex)
                {

                    // If the delay flag is down, it is forcibly delayed by one frame.
                    // The lowest bit in the aidBits in the transmission data is a delay flag.
                    if (!(context->ds[context->readIndex].aidBits & MPHToMP16(0x0001)))
                    {
                        context->ds[context->readIndex].aidBits |= MPHToMP16(0x0001);
                    }
                    else
                    {
                        // Read processing
                        // Because readIndex is only manipulated here, there is no need to disable interrupts
                        MPDSiTrace("read ds   : write: %d, read: %d, send: %d, seq#: %d",
                                   context->writeIndex, context->readIndex, context->sendIndex,
                                   context->seqNum[context->readIndex]);
                        (void)NETMemCpy(recvDataSet, &context->ds[context->readIndex], sizeof(MPDSDataSet));
                        context->currentSeqNum = context->seqNum[context->readIndex];
                        context->readIndex = GetNextIndex(context->readIndex);

                        sendFlag = TRUE;
                        result = MP_RESULT_OK;
                    }
                }
                if ( isSync && result == MP_RESULT_NO_DATA )
                {
                    // If called in synchronous mode, and there was no received data, then sleep.
                    OSSleepThread(&context->readThreadQueue);
                }
                else
                {
                    break;
                }
            }
            if ( context->state != MPDS_STATE_START )
            {
                result = MP_RESULT_ILLEGAL_STATE;
            }
        }

        if (sendFlag)
        {
            // The child transmits it as-is
            int     res;
            // Take the liberty of using part of context->ds as a send buffer
            u16    *buf = (u16 *)(((u8 *)&context->ds[context->sendIndex]) + 32); // 32-byte alignment is required

            MPDSiTrace("send data : write: %d, read: %d, send: %d", context->writeIndex,
                                 context->readIndex, context->sendIndex);

            (void)NETMemCpy(buf, sendData, context->config.dataLength);
            res =
                MPSendAsync( buf, context->dataSetLength,
                             context->config.aidBits, context->config.port,
                             NULL, SetDataCallback, (void*)context );
            context->sendIndex = GetNextIndex(context->sendIndex);
            SignalDataSet(context);
            if (res != MP_RESULT_OK)
            {
                MPDSiWarning("MPSendAsync failed during data-sharing. result=%x", res);
                context->state = MPDS_STATE_ERROR;
                result = res;
            }
        }
    }
#endif

    return result;
}

/*---------------------------------------------------------------------------*
  Name        : MPDSGetData
  Description : Gets the data that was shared
  Arguments   : context - The data sharing context structure
                dataSet - a shared data structure
                aid     - the aid of the data to be obtained
  Returns     : u8*     - a pointer to the shared data
 *---------------------------------------------------------------------------*/
const u8* MPDSGetData( const MPDSContext* context, const MPDSDataSet* dataSet, u32 aid )
{
    u16     aidBits;
    u16     receivedBits;
    u16     aidBit = (u16)(1U << aid);

    // Check parameters
    if (context == NULL || dataSet == NULL)
    {
        MPDSiWarning("Invalid parameter");
        return NULL;
    }

    aidBits = MPHToMP16(dataSet->aidBits);
    receivedBits = MPHToMP16(dataSet->receivedBits);

    if (!(aidBits & aidBit))
    {
        return NULL;
    }

    if (!(receivedBits & aidBit))
    {
        // Has not received data
        return NULL;
    }

    return GetSharedDataAddress(context, aidBits, (u16*)dataSet->data, aid);
}

/*---------------------------------------------------------------------------*
  Name:         SetDataCallback

  Description:  Send completion callback.

  Arguments:    callback:   A pointer to the callback structure.

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void SetDataCallback(s32 result, MPCallbackInfo *info)
{
#pragma unused(result)
    MPDSContext *context = (MPDSContext *)info->userData;

    if ( context == NULL )
    {
        MPDSiWarning("data-sharing has already terminated.");
        return;
    }

    if ( info->result == MP_RESULT_OK )
    {
        // Send completed
        if ( context->config.isParent )
        {
            // Parent-side processing
            MPDSiTrace("sent ds   : write: %d, read: %d, send: %d", context->writeIndex,
                                 context->readIndex, context->sendIndex);

            context->seqNum[context->sendIndex] = (u16)(info->dataSent.seqNo >> 1);
            context->sendIndex = GetNextIndex(context->sendIndex);
            SignalDataSet(context);
        }
#ifdef MPDS_ENABLE_CHILD
        else
        {
            // Child-side processing
            MPDSiTrace("sent data : write: %d, read: %d, send: %d", context->writeIndex,
                                 context->readIndex, context->sendIndex);
        }
#endif
    }
    else
    {
        MPDSiWarning("MPSendAsync failed during data-sharing. result=%x",
                     info->result);
        context->state = MPDS_STATE_ERROR;
    }
}

/*---------------------------------------------------------------------------*
  Name:         ReceiveCallback_Parent

  Description:  A parent port receive callback

  Arguments:    callback:   A pointer to the callback structure.

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void ReceiveCallback_Parent(s32 type, MPPortCallbackInfo *info)
{
#pragma unused(type)
    MPDSContext *context = (MPDSContext *)info->userData;

    if (context == NULL)
    {
        MPDSiWarning("data-sharing has already terminated.");
        return;
    }

    if ( info->result == MP_RESULT_OK )
    {
        switch (info->type)
        {
        case MP_PORT_CB_TYPE_DATA_RECEIVED:
            MPDSiTrace("data received to MPDS: aid=%d, len=%d, seqNo=%d", info->dataReceived.fromAid, info->dataReceived.length, info->dataReceived.seqNo);
            // Store the received child data in a buffer
            ReceiveData(context, info->dataReceived.fromAid, info->dataReceived.data);
            SendDataSet(context, FALSE);
            break;

        case MP_PORT_CB_TYPE_CONNECTED:
            MPDSiTrace("connected notification to MPDS: aid=%d", info->connected.fromAid);
            SendDataSet(context, FALSE);    // Should not be required
            {
                MPPortCallback cb = context->config.mpdsCallback;
                if ( cb != NULL )
                {
                    cb(type, info);
                }
            }
            break;

        case MP_PORT_CB_TYPE_DISCONNECTED:
            MPDSiTrace("disconnected notification to MPDS: aid=%d", info->disconnected.fromAid);
            {
                u32     aidBit;
                u32     writeIndex;
                BOOL    enabled;
                aidBit = 1U << info->dataReceived.fromAid;
                enabled = OSDisableInterrupts();
                writeIndex = context->writeIndex;
                MPDSiLOG_AIDBITS_BEGIN( context, writeIndex );
                context->ds[writeIndex].aidBits &= MPHToMP16((u16)~aidBit);
                MPDSiLOG_AIDBITS_END( context, writeIndex );
                if (context->config.isDoubleMode == TRUE)
                {
                    MPDSiLOG_AIDBITS_BEGIN( context, GetNextIndex(writeIndex) );
                    context->ds[GetNextIndex(writeIndex)].aidBits &= MPHToMP16((u16)~aidBit);
                    MPDSiLOG_AIDBITS_END( context, GetNextIndex(writeIndex) );
                }
                (void)OSRestoreInterrupts(enabled);
                SendDataSet(context, FALSE);
                if (context->config.isDoubleMode == TRUE)
                {
                    SendDataSet(context, FALSE);
                }
            }
            {
                MPPortCallback cb = context->config.mpdsCallback;
                if ( cb != NULL )
                {
                    cb(type, info);
                }
            }
            break;

        case MP_PORT_CB_TYPE_STARTUP:
            MPDSiTrace("startup notification to MPDS.");
            // Note that notifications that can be received are done within Start
            {
                MPPortCallback cb = context->config.mpdsCallback;
                if ( cb != NULL )
                {
                    cb(type, info);
                }
            }
            if (context->config.isAutoStart)
            {
                Start(context);
            }
            break;

        case MP_PORT_CB_TYPE_CLEANUP:
            MPDSiTrace("cleanup notification to MPDS.");
            if (context->config.isAutoStart)
            {
                Stop(context);
            }
            {
                MPPortCallback cb = context->config.mpdsCallback;
                if ( cb != NULL )
                {
                    cb(type, info);
                }
            }
            break;
        }
    }
    else
    {
        MPDSiWarning("An unknown receiving error occured during data-sharing. result=%x",
                     info->result);
        context->state = MPDS_STATE_ERROR;
    }
}

/*---------------------------------------------------------------------------*
  Name:         ReceiveData

  Description:  Stores the data, because the parent received data from several devices.

  Arguments:    context - MPDSContext structure.
                aid:      AID of the terminal that received data.
                data:     A pointer to the received data.

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void ReceiveData(MPDSContext* context, u32 aid, u8* data)
{
    u16     aidBit = (u16)(1 << aid);

    MPDSiTrace("recv data%d: write: %d, read: %d, send: %d", aid, context->writeIndex,
                         context->readIndex, context->sendIndex);

    // Confirms whether it is a processing target
    if (context->config.aidBits & aidBit)
    {
        u8*     buf;
        u16     writeIndex;
        BOOL    enabled;

        if (!(context->ds[context->writeIndex].aidBits & MPHToMP16(aidBit)))
        {
            if (context->config.isDoubleMode == TRUE)
            {
                MPDSiReport("[DS] received two DS packets from aid %d", aid);

                writeIndex = GetNextIndex(context->writeIndex);
                if (!(context->ds[writeIndex].aidBits & MPHToMP16(aidBit)))
                {
                    // Up to two are stored in the buffer. Anything beyond that is discarded.
                    MPDSiWarning("received too many DataSharing packets from aid %d. discarded.",
                               aid);
                    return;
                }
            }
            else
            {
                // If not in doubleMode, the buffer can hold only up to one
                MPDSiWarning("received too many DataSharing packets from aid %d. discarded.", aid);
                return;
            }
        }
        else
        {
            writeIndex = context->writeIndex;
        }

        buf = GetSharedDataAddress(context, context->config.aidBits, context->ds[writeIndex].data, aid);
        if (data != NULL)
        {
            (void)NETMemCpy(buf, data, context->config.dataLength);
        }
        else
        {
            MPDSiWarning( "received data is null." );
            (void)NETMemSet(buf, 0, context->config.dataLength);
        }

        enabled = OSDisableInterrupts();
        // Disables the not-yet-received flag
        MPDSiLOG_AIDBITS_BEGIN( context, writeIndex );
        context->ds[writeIndex].aidBits &= MPHToMP16((u16)~aidBit);
        MPDSiLOG_AIDBITS_END( context, writeIndex );
        // Enables the received flag
        context->ds[writeIndex].receivedBits |= MPHToMP16(aidBit);
        (void)OSRestoreInterrupts(enabled);
    }
}

/*---------------------------------------------------------------------------*
  Name:         SendDataSet

  Description:  After the parent confirms reception of each device's data, the data set is sent.

  Arguments:    context - MPDSContext structure.
                delayed:   TRUE if in frame delay state

  Returns:      None.
 *---------------------------------------------------------------------------*/
void SendDataSet(MPDSContext *context, BOOL delayed)
{
    BOOL enabled;

    enabled = OSDisableInterrupts();
    // If and when reception has completed for all devices
    // (The ds[].aidBits acts as unreceived flag during the receiving buffer)
    if (context->ds[context->writeIndex].aidBits == MPHToMP16(0))
    {
        u16     newWI, oldWI, resetWI;
        s32     res;
        u32     mpReadyBits;

        MPDSiTrace("send ds   : write: %d, read: %d, send: %d", context->writeIndex,
                             context->readIndex, context->sendIndex);

        mpReadyBits = MPGetConnectedAIDs();
        oldWI = context->writeIndex;
        newWI = GetNextIndex(oldWI);
        if (context->config.isDoubleMode == TRUE)
        {
            resetWI = GetNextIndex(newWI);
        }
        else
        {
            resetWI = newWI;
        }
        ASSERT(newWI != context->readIndex && resetWI != context->readIndex);
//        ASSERT( context->sendIndex == context->writeIndex);
        (void)NETMemSet(&context->ds[resetWI], 0, sizeof(MPDSDataSet));
        MPDSiLOG_AIDBITS_BEGIN( context, resetWI );
        context->ds[resetWI].aidBits = MPHToMP16((u16)(context->config.aidBits & (mpReadyBits | 0x0001)));
        MPDSiLOG_AIDBITS_END( context, resetWI );
        context->writeIndex = newWI;
        MPDSiLOG_AIDBITS_BEGIN( context, oldWI );
        context->ds[oldWI].aidBits = MPHToMP16((u16)context->config.aidBits);        // Enter the original value in aidBits before sending
        if (delayed == TRUE)
        {
            // The lowest bit in the aidBits is a delay flag
            context->ds[oldWI].aidBits &= MPHToMP16((u16)~0x0001);
        }
        MPDSiLOG_AIDBITS_END( context, oldWI );
        (void)OSRestoreInterrupts(enabled);
        res =
            MPSendAsync( &context->ds[oldWI], context->dataSetLength,
                         (u16)(context->config.aidBits & mpReadyBits), context->config.port,
                         NULL, SetDataCallback, (void*)context );
        if (res != MP_RESULT_OK)
        {
            MPDSiWarning("MPSendAsync failed during data-sharing. result=%x", res);
            context->state = MPDS_STATE_ERROR;
        }
    }
    else
    {
        (void)OSRestoreInterrupts(enabled);
    }
}

/*---------------------------------------------------------------------------*
  Name:         GetSharedDataAddress

  Description:  Gets a specific AID address within the receive buffer.

  Arguments:    context     -   MPDSContext structure
                aidBits   -   Other communication peer(s) included in the data
                receiveBuf:   Receive buffer
                aid:           AID.
                                Confirm aidBits & (1<<aid) before calling this.

  Returns:      u16* :     The address of the received data.
 *---------------------------------------------------------------------------*/
u8*
GetSharedDataAddress( const MPDSContext *context, u32 aidBits, u16 *receiveBuf, u32 aid)
{
    u32     mask;
    u32     count;
    u32     offset;

    // Count the 1's among the aid bits in the aidBits, from the bottom
    mask = (0x0001U << aid) - 1U;
    aidBits &= mask;
    count = CountPopulation(aidBits);
    offset = context->config.dataLength * count;

    return ((u8 *)receiveBuf) + offset;
}

#ifndef NDEBUG
/*---------------------------------------------------------------------------*
  Name        : MPDSSetDebugLevel
  Description : Change the level of debug output.
  Arguments   : level - Debug output type bit to be output
  Returns     : None.
 *---------------------------------------------------------------------------*/
void
MPDSSetDebugLevel( u32 level )
{
    MPDSi_debugLevel = level;
}

/*---------------------------------------------------------------------------*
  Name        : MPDSiVLogCore
  Description : Outputs a log.
  Arguments   : level   - Output level of the log.
                levelString - String notation for the output level.
                file  - Filename.
                func    - Function name.
                line    - Line number in file.
                fmt     - Text string to be output.
                vlist   - Variable-length arguments.
  Returns     : None.
 *---------------------------------------------------------------------------*/
void
MPDSiVLogCore( u32 level, const char* levelString,
               const char* file, const char* func, int line, const char* fmt, va_list vlist )
{
#pragma unused( func )

    if( ! MPDSi_IS_DEBUG_LEVEL(level) )
    {
        return;
    }

    {
        OSReport( "%c[%04x|MPDS %s] ",
                  (level & MPDSi_DEBUG_LEVEL_IMPORTANTS) ? '!' : ' ',
                  (u32)(OSTicksToMilliseconds(OSGetTick())&0xffff),
                  levelString );
        if( fmt != NULL )
        {
            OSVReport( fmt, vlist );
        }
        OSReport( " (%s:%d)\n", file, line );
    }
}

/*---------------------------------------------------------------------------*
  Name        : MPDSiLogCore
  Description : Outputs a log.
  Arguments   : level   - Output level of the log.
                levelString - String notation for the output level.
                file  - Filename.
                func    - Function name.
                line    - Line number in file.
                fmt     - Text string to be output.
  Returns     : None.
 *---------------------------------------------------------------------------*/
void
MPDSiLogCore( u32 level, const char* levelString,
              const char* file, const char* func, int line, const char* fmt, ... )
{
    va_list     vlist;

    va_start( vlist, fmt );
    MPDSiVLogCore( level, levelString, file, func, line, fmt, vlist );
    va_end( vlist );
}
#endif // !NDEBUG


u8 CountPopulation( u32 x )
{
    // Rather than counting 32 bits directly, first store the number of 1's for every 2 bits in the same location as those 2 bits.
    // 
    // In other words, every 2 bits are converted such that 00 -> 00, 01 -> 01, 10 -> 01, and 11 -> 10.
    // When x -> x', for a 2-bit value we have x' = x - (x >> 1).
    x -= ((x >> 1) & 0x55555555);

    // When counting in 4-bit units, add the number of 1's stored in the upper and lower 2 bits, and then store this as the number of 1's in that original 4-bit location.
    // 
    x = (x & 0x33333333) + ((x >> 2) & 0x33333333);

    // Do the same for 8-bit units.
    // However, the maximum result for each digit is 8, and this fits in 4 bits, it is not necessary to mask ahead of time.
    // 
    x += (x >> 4);

    // Mask unnecessary parts in preparation for the next operations.
    x &= 0x0f0f0f0f;

    // Get the sum of the upper 8 bits and lower 8 bits in 16-bit units.
    x += (x >> 8);

    // Do the same for 32-bit units.
    x += (x >> 16);

    // The lower 8-bit value is the result.
    return (u8)( x & 0xff );
}

/*---------------------------------------------------------------------------*
  End of file
 *---------------------------------------------------------------------------*/