/*---------------------------------------------------------------------------*
  Project:  NitroSDK - WM - libraries
  File:     wm_ds.c

  Copyright 2003,2004 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: wm_ds.c,v $
  Revision 1.9  2004/10/29 03:36:03  seiki_masashi
  Fixed debug messages

  Revision 1.8  2004/10/29 01:41:11  seiki_masashi
  Supported for send competion callback after WM_EndDataSharing.

  Revision 1.7  2004/10/22 04:37:28  terui
  Changed functions that return int type as WMErrCode so that they return WMErrCode enumerator type.

  Revision 1.6  2004/09/23 10:02:10  seiki_masashi
  small fix.

  Revision 1.5  2004/09/23 09:47:31  seiki_masashi
  Added a DataSharing internal error processing routine 

  Revision 1.4  2004/09/22 04:06:43  seiki_masashi
  Corrected an error related to DataSharing exclusion 

  Revision 1.3  2004/09/21 10:50:38  takano_makoto
  remove warning in WM_GetSharedDataAddress()

  Revision 1.2  2004/09/10 12:02:46  terui
  Bug fix ( port number check ).

  Revision 1.1  2004/09/10 11:03:30  terui
  New upload reflecting breakup of wm.c. 

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

#include    <nitro/wm.h>
#include    "wm_arm9_private.h"


/*---------------------------------------------------------------------------*
    Internal function definitions
 *---------------------------------------------------------------------------*/
static void WmDataSharingSetDataCallback( void* callback );
static void WmDataSharingReceiveCallback_Parent( void* callback );
static void WmDataSharingReceiveCallback_Child( void* callback );
static void WmDataSharingReceiveData( WMDataSharingInfo* dsInfo, u16 aid, u16* data );
static void WmDataSharingSendDataSet( WMDataSharingInfo* dsInfo, BOOL delayed );
static u16* WmGetSharedDataAddress( WMDataSharingInfo* dsInfo, u16 aidBitmap, u16* receiveBuf, u16 aid );


/*---------------------------------------------------------------------------*
  Name:         WM_StartDataSharing

  Description:  Enables the data sharing function.
                By MP communicating after enabling the function, data sharing
                communication occurs accompanied by MP communication.

  Arguments:    dsInfo      -   WMDataSharingInfo structure
                port        -   port number to use
                aidBitmap   -   AID of partner that does data sharing
                dataLength  -   shared memory data length (specify an even of bytes)
                doubleMode  -   When WM_StepDataSharing done each frame, specify TRUE

  Returns:      WMErrCode   -   Returns process results. WM_ERRCODE_OPERATING is returned
  					when an asynchronous process started normally. After that, asynchronous
                                	process results are again passed to callback.
 *---------------------------------------------------------------------------*/
WMErrCode
WM_StartDataSharing( WMDataSharingInfo* dsInfo, u16 port, u16 aidBitmap, u16 dataLength, BOOL doubleMode )
{
    WMErrCode   result;
    int         aid;
    u16         mpReadyBitmap = 0x0001;
    WMArm9Buf*  p   = WMi_GetSystemWork();

    // Check status
    result = WMi_CheckStateEx( 2 , WM_STATE_MP_PARENT , WM_STATE_MP_CHILD );
    WM_CHECK_RESULT( result );

    // Confirm MP status
    DC_InvalidateRange( &( p->status->mp_flag ) , 4 );
    if( p->status->mp_flag == FALSE )
    {
        WM_WARNING( "It is not MP mode now.\n" );
        return WM_ERRCODE_ILLEGAL_STATE;
    }

    //Check parameters
    if( dsInfo == NULL )
    {
        WM_WARNING( "Parameter \"dsInfo\" must not be NULL.\n" );
        return WM_ERRCODE_INVALID_PARAM;
    }
    if( port >= WM_NUM_OF_PORT )
    {
        WM_WARNING( "Parameter \"port\" must be less than %d.\n" , WM_NUM_OF_PORT );
        return WM_ERRCODE_INVALID_PARAM;
    }
    if( (u32)dsInfo & 0x01f )
    {
        // For alignment checks, only warnings are used, not errors 
        WM_WARNING( "Parameter \"dsInfo\" is not 32-byte aligned.\n" );
    }
    if ( aidBitmap == 0 )
    {
        WM_WARNING( "Parameter \"aidBitmap\" must not be 0.\n" );
        return WM_ERRCODE_INVALID_PARAM;
    }

    // Get the required info from WMStatus structure
    DC_InvalidateRange( &( p->status->aid ) , 2 );
    aid = p->status->aid;
    if( aid == 0 )
    {
        DC_InvalidateRange( &( p->status->mp_readyBitmap ) , 2 );
        mpReadyBitmap = p->status->mp_readyBitmap;
    }

    // Initialize data sharing control variables
    MI_CpuClearFast( dsInfo, sizeof(WMDataSharingInfo) );
    dsInfo->writeIndex  = 0;
    dsInfo->sendIndex   = 0;
    dsInfo->readIndex   = 0;
    dsInfo->dataLength  = dataLength;
    dsInfo->port        = port;
    dsInfo->aidBitmap   = 0;
    dsInfo->doubleMode  = (u16)((doubleMode) ? TRUE : FALSE);

    aidBitmap |= ( 1 << aid ); // Adds to own portion 
    dsInfo->aidBitmap = aidBitmap;

    {
        u16 count = 0;
        u16 iAid;
        u16 mask = 0x0001;
        for ( iAid = 0; iAid < 16; iAid++ )
        {
            if ( aidBitmap & mask )
            {
                count++;
            }
            mask <<= 1;
        }
        
        dsInfo->stationNumber = count;
        dsInfo->dataSetLength = (u16)(dataLength * count);
        
        if ( dsInfo->dataSetLength > WM_DS_DATA_SIZE )
        {
            // Total data amount exceeds limit 
            dsInfo->aidBitmap = 0;
            WM_WARNING( "Total size of sharing data must be less than or equal to %d bytes.\n", WM_DS_DATA_SIZE );
            return WM_ERRCODE_INVALID_PARAM;
        }
        dsInfo->dataSetLength += 4; // aidBitmap, receivedBitmap
    }

    dsInfo->state = WM_DS_STATE_START;

    if ( aid == 0 )
    {
        // Parent start processing 
        int i;
        
        for ( i = 0; i < WM_DS_DATASET_NUM; i++ )
        {
            dsInfo->ds[i].aidBitmap = (u16)(dsInfo->aidBitmap & (mpReadyBitmap|0x0001));
        }
        
        (void) WM_SetPortCallback( port, WmDataSharingReceiveCallback_Parent, (void*)dsInfo );
        
        // Sends empty data for start-up 
        for ( i = 0; i < ((dsInfo->doubleMode == TRUE) ? 2 : 1); i++ )
        {
            int res;
            
            dsInfo->writeIndex = (u16)((dsInfo->writeIndex + 1) % WM_DS_DATASET_NUM);
            res = WM_SetMPDataToPortEx( WmDataSharingSetDataCallback, dsInfo, (u16*)&dsInfo->ds[i], dsInfo->dataSetLength, (u16)(dsInfo->aidBitmap&mpReadyBitmap), dsInfo->port, WM_PRIORITY_HIGH );
            if ( res == WM_ERRCODE_NO_CHILD )
            {
                dsInfo->seqNum[i] = 0xffff;
                dsInfo->sendIndex = (u16)((dsInfo->sendIndex + 1) % WM_DS_DATASET_NUM);
            }
            else {
                if ( res != WM_ERRCODE_SUCCESS && res != WM_ERRCODE_OPERATING )
                {
                    WM_WARNING( "WM_SetMPDataToPort failed during data-sharing. errcode=%x\n", res );
                    dsInfo->state = WM_DS_STATE_ERROR;
                    return WM_ERRCODE_FAILED;
                }
            }
        }
    }
    else
    {
        // Child start processing 
        dsInfo->sendIndex = (u16)(WM_DS_DATASET_NUM - 1);
        (void)WM_SetPortCallback( port, WmDataSharingReceiveCallback_Child, (void*)dsInfo );
    }

    return WM_ERRCODE_SUCCESS;  // Successfully ended 
}

/*---------------------------------------------------------------------------*
  Name:         WM_EndDataSharing

  Description:  Disables data sharing function.

  Arguments:    dsInfo      -   WMDataSharingInfo structure

  Returns:      WMErrCode   -   Returns process results.
 *---------------------------------------------------------------------------*/
WMErrCode
WM_EndDataSharing( WMDataSharingInfo* dsInfo )
{
    WMErrCode   result;
    WMArm9Buf*  p   = WMi_GetSystemWork();

    // State check 
    result = WMi_CheckStateEx( 2 , WM_STATE_MP_PARENT , WM_STATE_MP_CHILD );
    WM_CHECK_RESULT( result );

    // Parameter check 
    if( dsInfo == NULL )
    {
        WM_WARNING( "Parameter \"dsInfo\" must not be NULL.\n" );
        return WM_ERRCODE_INVALID_PARAM;
    }

    // Checks the data sharing state  
    if ( dsInfo->aidBitmap == 0
        || (p->portCallbackTable[ dsInfo->port ] != WmDataSharingReceiveCallback_Parent
         && p->portCallbackTable[ dsInfo->port ] != WmDataSharingReceiveCallback_Child) )
    {
        WM_WARNING( "It is not data-sharing mode now.\n" );
        return WM_ERRCODE_ILLEGAL_STATE;
    }

    (void)WM_SetPortCallback( dsInfo->port, NULL, NULL );
    dsInfo->aidBitmap = 0;
    dsInfo->state = WM_DS_STATE_READY;

    return WM_ERRCODE_SUCCESS;  // Successfully ended 
}

/*---------------------------------------------------------------------------*
  Name:         WM_StepDataSharing

  Description:  advances synchronicity of data sharing by one

  Arguments:    dsInfo      -   WMDataSharingInfo structure
                sendData    -   send data you want to share
                receiveData -   shared data received

  Returns:      WMErrCode   -   Returns process results.
                         *_SUCCESS      : shared data send/receive success
                         *_NO_DATASET   : shared data has not come yet
 *---------------------------------------------------------------------------*/
WMErrCode
WM_StepDataSharing( WMDataSharingInfo* dsInfo, const u16* sendData, WMDataSet* receiveData )
{
    WMErrCode   result;
    u16         aid;
    u16         mpReadyBitmap;
    WMArm9Buf*  p   = WMi_GetSystemWork();
    u16         state;

    // State check 
    result = WMi_CheckStateEx( 2 , WM_STATE_MP_PARENT , WM_STATE_MP_CHILD );
    WM_CHECK_RESULT( result );

    // MP state confirmation 
    DC_InvalidateRange( &( p->status->mp_flag ) , 4 );
    if( p->status->mp_flag == FALSE )
    {
        WM_WARNING( "It is not MP mode now.\n" );
        return WM_ERRCODE_ILLEGAL_STATE;
    }

    // Parameter check 
    if( dsInfo == NULL )
    {
        WM_WARNING( "Parameter \"dsInfo\" must not be NULL.\n" );
        return WM_ERRCODE_INVALID_PARAM;
    }
    if( sendData == NULL )
    {
        WM_WARNING( "Parameter \"sendData\" must not be NULL.\n" );
        return WM_ERRCODE_INVALID_PARAM;
    }
    if( receiveData == NULL )
    {
        WM_WARNING( "Parameter \"receiveData\" must not be NULL.\n" );
        return WM_ERRCODE_INVALID_PARAM;
    }

    // Gets the required information from the WMStatus structure 
    DC_InvalidateRange( &( p->status->aid ) , 2 );
    aid = p->status->aid;
    if( aid == 0 )
    {
        DC_InvalidateRange( &( p->status->mp_readyBitmap ) , 2 );
        mpReadyBitmap = p->status->mp_readyBitmap;
    }

    // Checks the data sharing state 
    state = dsInfo->state;
    if ( state == WM_DS_STATE_ERROR )
    {
        WM_WARNING( "An error occurred during data-sharing.\n" );
        return WM_ERRCODE_FAILED;
    }
    if ( state != WM_DS_STATE_START && state != WM_DS_STATE_RETRY_SEND )
    {
        WM_WARNING( "It is not data-sharing mode now.\n" );
        return WM_ERRCODE_ILLEGAL_STATE;
    }

    result = WM_ERRCODE_NO_DATASET;

    if ( aid == 0 )
    {
        // If a parent 
        BOOL sendFlag = FALSE;
        BOOL delayed = FALSE;
        
        if ( state == WM_DS_STATE_RETRY_SEND )
        {
            // Resends for the previous SEND_QUEUE_FULL 
            int res;
            int oldWI;
            dsInfo->state = WM_DS_STATE_START;
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| send queue was full. do retry.\n", (OS_GetVBlankCount()&0xff), GX_GetVCount() );
#endif
            oldWI = (u16)((dsInfo->writeIndex + WM_DS_DATASET_NUM - 1) % WM_DS_DATASET_NUM);
            res = WM_SetMPDataToPortEx( WmDataSharingSetDataCallback, dsInfo, (u16*)&dsInfo->ds[oldWI], dsInfo->dataSetLength, (u16)(dsInfo->aidBitmap&mpReadyBitmap), dsInfo->port, WM_PRIORITY_HIGH );
            if ( res == WM_ERRCODE_NO_CHILD )
            {
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| sent ds   : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
                dsInfo->seqNum[oldWI] = 0xffff;
                dsInfo->sendIndex = (u16)((dsInfo->sendIndex + 1) % WM_DS_DATASET_NUM);
            }
            else
            {
                if ( res != WM_ERRCODE_SUCCESS && res != WM_ERRCODE_OPERATING )
                {
                    WM_WARNING( "WM_SetMPDataToPort failed during data-sharing. errcode=%x\n", res );
                    dsInfo->state = WM_DS_STATE_ERROR;
                    return WM_ERRCODE_FAILED;
                }
            }
        }
        
        // When the parent finishes sending the DataSet, this is enabled, so a limit is set for reading the sendIndex 
        if ( dsInfo->readIndex != dsInfo->sendIndex )
        {
            // Read processing 
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| read ds   : write: %d, read: %d, send: %d, seq#: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex, dsInfo->seqNum[dsInfo->readIndex] );
#endif
            dsInfo->ds[dsInfo->readIndex].aidBitmap |= 0x0001; // The lowest bit in the aidBitmap in the transmission data is a delay flag 
            MI_CpuCopy16( &dsInfo->ds[dsInfo->readIndex], receiveData, sizeof(WMDataSet) );
            dsInfo->currentSeqNum = dsInfo->seqNum[dsInfo->readIndex];
            dsInfo->readIndex = (u16)((dsInfo->readIndex + 1) % WM_DS_DATASET_NUM);
            
            sendFlag = TRUE;
            result = WM_ERRCODE_SUCCESS;
            if ( dsInfo->doubleMode == FALSE && mpReadyBitmap != 0
                && dsInfo->ds[dsInfo->writeIndex].aidBitmap == 0x0001 )
            {
                // If the parent sets the data, the frame gets out of alignment only when transferrable = parent 
                delayed = TRUE;
            }
            else
            {
                delayed = FALSE;
            }
        }
        
        // Sends the DataSet to the transmission buffer when the data for all the machines is ready 
        WmDataSharingSendDataSet( dsInfo, FALSE );
        
        if ( sendFlag )
        {
            // The parent inserts into its own buffer instead of sending 
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| send data : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
            WmDataSharingReceiveData( dsInfo, 0, (u16*)sendData ); // Removes the const because the previous rewrite chain is troublesome 
            
            if ( dsInfo->doubleMode == FALSE )
            {
                // Only when not a Step in each frame, 
                // the DataSet needs to be prepared for sending with this GF at this point in time.
                // Sends the DataSet to the transmission buffer when the data for all the machines is ready 
                WmDataSharingSendDataSet( dsInfo, delayed );
            }
        }
    }
    else
    {
        // If a child 
        BOOL sendFlag = FALSE;

        if ( state == WM_DS_STATE_RETRY_SEND )
        {
            // Resends for the previous SEND_QUEUE_FULL 
            sendFlag = TRUE;
            dsInfo->state = WM_DS_STATE_START;
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| send queue was full. do retry.\n", (OS_GetVBlankCount()&0xff), GX_GetVCount() );
#endif
        }
        else
        {
            // When the child receives the DataSet, it is immediately usable, so a limit is set for reading writeIndex 
            if ( dsInfo->readIndex != dsInfo->writeIndex )
            {
                // If the delay flag is down, one frame is forcibly delayed 
                // The lowest bit in the aidBitmap in the transmission data is a delay flag 
                if ( ! (dsInfo->ds[dsInfo->readIndex].aidBitmap & 0x0001) )
                {
                    dsInfo->ds[dsInfo->readIndex].aidBitmap |= 0x0001;
                }
                else
                {
                    // Read processing 
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| read ds   : write: %d, read: %d, send: %d, seq#: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex, dsInfo->seqNum[dsInfo->readIndex] );
#endif
                    MI_CpuCopy16( &dsInfo->ds[dsInfo->readIndex], receiveData, sizeof(WMDataSet) );
                    dsInfo->currentSeqNum = dsInfo->seqNum[dsInfo->readIndex];
                    dsInfo->readIndex = (u16)((dsInfo->readIndex + 1) % WM_DS_DATASET_NUM);
                    
                    sendFlag = TRUE;
                    result = WM_ERRCODE_SUCCESS;
                }
            }
        }
        
        if ( sendFlag )
        {
            // The child transmits as is 
            int res;
            // A portion of dsInfo -> ds is used as a transmission buffer 
            u16* buf = (u16*)(((u8*)&dsInfo->ds[dsInfo->sendIndex]) + 32); // 32-byte alignment is required 
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| send data : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
            MI_CpuCopy16( sendData, buf, dsInfo->dataLength );
            res = WM_SetMPDataToPortEx( WmDataSharingSetDataCallback, dsInfo, buf, dsInfo->dataLength, dsInfo->aidBitmap, dsInfo->port, WM_PRIORITY_HIGH );
            dsInfo->sendIndex = (u16)((dsInfo->sendIndex + 1) % WM_DS_DATASET_NUM);
            if ( res != WM_ERRCODE_OPERATING && res != WM_ERRCODE_SUCCESS )
            {
                WM_WARNING( "WM_SetMPDataToPort failed during data-sharing. errcode=%x\n", res );
                dsInfo->state = WM_DS_STATE_ERROR;
                result = WM_ERRCODE_FAILED;
            }
        }
    }

    return result;
}

/*---------------------------------------------------------------------------*
  Name:         WmDataSharingSetDataCallback

  Description:  A transmission completion callback. 

  Arguments:    callback - A pointer to the callback structure. 

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void
WmDataSharingSetDataCallback( void* callback )
{
    WMArm9Buf*          p   = WMi_GetSystemWork();
    WMPortSendCallback* cb_Port = (WMPortSendCallback*)callback;
    WMDataSharingInfo* dsInfo;
    u16 aid;

    dsInfo = (WMDataSharingInfo*)( p->portCallbackArgument[cb_Port->port] );
    if ( ( p->portCallbackTable[ cb_Port->port ] != WmDataSharingReceiveCallback_Parent
         && p->portCallbackTable[ cb_Port->port ] != WmDataSharingReceiveCallback_Child )
         || dsInfo == NULL || dsInfo != (WMDataSharingInfo*)( cb_Port->arg ) )
    {
        WM_WARNING( "data-sharing has already terminated." );
        return;
    }
    
    DC_InvalidateRange( &( p->status->aid ) , 2 );
    aid = p->status->aid;
    
    if ( cb_Port->errcode == WM_ERRCODE_SUCCESS )
    {
        // send completion
        if( aid == 0 )
        {
            // Parent processing 
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| sent ds   : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
            dsInfo->seqNum[dsInfo->sendIndex] = (u16)(cb_Port->seqNo >> 1);
            dsInfo->sendIndex = (u16)((dsInfo->sendIndex + 1) % WM_DS_DATASET_NUM);
        }
        else
        {
            // Child processing 
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| sent data : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
        }
    }
    else
    {
        if ( cb_Port->errcode == WM_ERRCODE_SEND_QUEUE_FULL )
        {
            // Resend as the transmission queue was full 
            if ( aid != 0 )
            {
                // For the child, put the sendIndex back one 
                dsInfo->sendIndex = (u16)((dsInfo->sendIndex + WM_DS_DATASET_NUM - 1) % WM_DS_DATASET_NUM);
            }
            dsInfo->state = WM_DS_STATE_RETRY_SEND;
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| send queue is full. will retry.\n", (OS_GetVBlankCount()&0xff), GX_GetVCount() );
#endif
        }
        else
        {
            WM_WARNING( "WM_SetMPDataToPort failed during data-sharing. errcode=%x\n", cb_Port->errcode );
            dsInfo->state = WM_DS_STATE_ERROR;
        }
    }
}

/*---------------------------------------------------------------------------*
  Name:         WmDataSharingReceiveCallback_Parent

  Description:  A parent port receiving callback 

  Arguments:    callback - A pointer to the callback structure. 

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void
WmDataSharingReceiveCallback_Parent( void* callback )
{
    WMPortRecvCallback* cb_Port = (WMPortRecvCallback*)callback;
    WMDataSharingInfo* dsInfo = (WMDataSharingInfo*)cb_Port->arg;

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

    if ( cb_Port->errcode == WM_ERRCODE_SUCCESS )
    {
        switch ( cb_Port->state )
        {
        case WM_STATECODE_PORT_RECV:
            // Stores the received child data in a buffer
            WmDataSharingReceiveData( dsInfo, cb_Port->aid, cb_Port->data );
            WmDataSharingSendDataSet( dsInfo, FALSE );
            break;
            
        case WM_STATECODE_CONNECTED:
            WmDataSharingSendDataSet( dsInfo, FALSE ); // This is most likely not necessary 
            break;
            
        case WM_STATECODE_DISCONNECTED:
            {
                u16 aidBit;
                OSIntrMode enabled;
                aidBit = (u16)(1 << cb_Port->aid);
                enabled = OS_DisableInterrupts();
                dsInfo->ds[dsInfo->writeIndex].aidBitmap &= ~aidBit;
                if ( dsInfo->doubleMode == TRUE )
                {
                    dsInfo->ds[(dsInfo->writeIndex + 1) % WM_DS_DATASET_NUM].aidBitmap &= ~aidBit;
                }
                (void)OS_RestoreInterrupts( enabled );
                WmDataSharingSendDataSet( dsInfo, FALSE );
                if ( dsInfo->doubleMode == TRUE )
                {
                    WmDataSharingSendDataSet( dsInfo, FALSE );
                }
            }
            break;
        }
    }
    else
    {
        WM_WARNING( "An unknown receiving error occured during data-sharing. errcode=%x\n", cb_Port->errcode );
        dsInfo->state = WM_DS_STATE_ERROR;
    }
}

/*---------------------------------------------------------------------------*
  Name:         WmDataSharingReceiveCallback_Child

  Description:  A child port receiving callback. 

  Arguments:    callback - A pointer to the callback structure. 

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void
WmDataSharingReceiveCallback_Child( void* callback )
{
    WMArm9Buf*      p   = WMi_GetSystemWork();
    WMPortRecvCallback* cb_Port = (WMPortRecvCallback*)callback;
    WMDataSharingInfo* dsInfo = (WMDataSharingInfo*)cb_Port->arg;

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

    if ( cb_Port->errcode == WM_ERRCODE_SUCCESS )
    {
        switch ( cb_Port->state )
        {
        case WM_STATECODE_PORT_RECV:
            {
                u16 length;
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| recv ds   : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
                // Saves the DataSet 
                length = cb_Port->length;
                if ( length != dsInfo->dataSetLength ) {
                    OS_Warning( "received DataSharing data size(%d) != dsInfo->dataSetLength(%d)\n", length, dsInfo->dataSetLength );
                    if ( length > sizeof(WMDataSet) )
                    {
                        OS_Warning( "received DataSharing data exceeds sizeof(WMDataSet)\n" );
                        length = sizeof(WMDataSet);
                    }
                }
                DC_InvalidateRange( &( p->status->aid ) , 2 );
                if ( length >= 4 && (((WMDataSet*)(cb_Port->data))->aidBitmap && (1 << p->status->aid)) )
                {
                    MI_CpuCopy16( cb_Port->data, &dsInfo->ds[dsInfo->writeIndex], length );
                    dsInfo->seqNum[dsInfo->writeIndex] = (u16)(cb_Port->seqNo >> 1);
                    dsInfo->writeIndex = (u16)((dsInfo->writeIndex + 1) % WM_DS_DATASET_NUM);
                }
            }
            break;
        case WM_STATECODE_CONNECTED:
        case WM_STATECODE_DISCONNECTED:
            break;
        }
    }
    else
    {
        WM_WARNING( "An unknown receiving error occured during data-sharing. errcode=%x\n", cb_Port->errcode );
        dsInfo->state = WM_DS_STATE_ERROR;
    }
}

/*---------------------------------------------------------------------------*
  Name:         WmDataSharingReceiveData

  Description:  Stores the data as the parent received data from each machine. 

  Arguments:    dsInfo - WMDataSharingInfo structure.
                aid    - AID for the terminal that received data. 
                data   - A pointer to the received data. 

  Returns:      None.
 *---------------------------------------------------------------------------*/
static void
WmDataSharingReceiveData( WMDataSharingInfo* dsInfo, u16 aid, u16* data )
{
    u16 aidBit = (u16)(1 << aid);

#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| recv data%d: write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), aid, dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
    // Confirms whether a processing target 
    if ( dsInfo->aidBitmap & aidBit )
    {
        u16* buf;
        u16 writeIndex;
        OSIntrMode enabled;
        
        if ( !(dsInfo->ds[dsInfo->writeIndex].aidBitmap & aidBit) )
        {
            if ( dsInfo->doubleMode == TRUE )
            {
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] received two DS packets from aid %d\n", aid );
#endif
                writeIndex = (u16)((dsInfo->writeIndex + 1) % WM_DS_DATASET_NUM);
                if ( !(dsInfo->ds[writeIndex].aidBitmap & aidBit) )
                {
                    // Up to two are stored in the buffer. Anything beyond that is discarded. 
                    OS_Warning( "received too many DataSharing packets from aid %d. discarded.\n", aid );
                    return;
                }
            }
            else
            {
                // If not in doubleMode, the buffer can hold up to one 
                OS_Warning( "received too many DataSharing packets from aid %d. discarded.\n", aid );
                return;
            }
        }
        else
        {
            writeIndex = dsInfo->writeIndex;
        }
        
        buf = WmGetSharedDataAddress( dsInfo, dsInfo->aidBitmap, dsInfo->ds[writeIndex].data, aid );
        if ( data != NULL )
        {
            MI_CpuCopy16( data, buf, dsInfo->dataLength );
        }
        else
        {
            MI_CpuClear16( buf, dsInfo->dataLength );
        }
        
        enabled = OS_DisableInterrupts();
        // Puts down the unreceived flag 
        dsInfo->ds[writeIndex].aidBitmap &= ~aidBit;
        // Puts up the received flag 
        dsInfo->ds[writeIndex].receivedBitmap |= aidBit;
        (void)OS_RestoreInterrupts( enabled );
    }
}

/*---------------------------------------------------------------------------*
  Name:         WmDataSharingSendDataSet

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

  Arguments:    dsInfo - WMDataSharingInfo structure
                delayed - TRUE if the frame delay state is set 

  Returns:      None.
 *---------------------------------------------------------------------------*/
void
WmDataSharingSendDataSet( WMDataSharingInfo* dsInfo, BOOL delayed )
{
    WMArm9Buf*  p   = WMi_GetSystemWork();
    OSIntrMode enabled;

    enabled = OS_DisableInterrupts();
    // If receiving is finished for all the machines,  
    // (The ds[].audBitmap acts as unreceived flag during the receiving buffer) 
    if ( dsInfo->ds[dsInfo->writeIndex].aidBitmap == 0 )
    {
        u16         newWI, oldWI, resetWI;
        WMErrCode   res;
        u16         mpReadyBitmap;
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| send ds   : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
        DC_InvalidateRange( &( p->status->mp_readyBitmap ) , 2 );
        mpReadyBitmap = p->status->mp_readyBitmap;
        oldWI = dsInfo->writeIndex;
        newWI = (u16)((oldWI + 1) % WM_DS_DATASET_NUM);
        if ( dsInfo->doubleMode == TRUE )
        {
            resetWI = (u16)((newWI + 1) % WM_DS_DATASET_NUM);
        }
        else
        {
            resetWI = newWI;
        }
        SDK_ASSERT( newWI != dsInfo->readIndex && resetWI != dsInfo->readIndex );
//        SDK_ASSERT( dsInfo->sendIndex == dsInfo->writeIndex);
        MI_CpuClear16( &dsInfo->ds[resetWI], sizeof(WMDataSet) );
        dsInfo->ds[resetWI].aidBitmap = (u16)(dsInfo->aidBitmap & (mpReadyBitmap|0x0001));
        dsInfo->writeIndex = newWI;
        dsInfo->ds[oldWI].aidBitmap = dsInfo->aidBitmap; // The value up to now is entered in the aidBitmap for transmission 
        if ( delayed == TRUE )
        {
            // The lowest bit in the aidBitmap is a delay flag 
            dsInfo->ds[oldWI].aidBitmap &= ~0x0001;
        }
        (void)OS_RestoreInterrupts( enabled );
        res = WM_SetMPDataToPortEx( WmDataSharingSetDataCallback, dsInfo, (u16*)&dsInfo->ds[oldWI], dsInfo->dataSetLength, (u16)(dsInfo->aidBitmap&mpReadyBitmap), dsInfo->port, WM_PRIORITY_HIGH );
        if ( res == WM_ERRCODE_NO_CHILD )
        {
#if WM_DEBUG_DATASHARING
OS_Printf( "[DS] %02x-%03d| sent ds   : write: %d, read: %d, send: %d\n", (OS_GetVBlankCount()&0xff), GX_GetVCount(), dsInfo->writeIndex, dsInfo->readIndex, dsInfo->sendIndex );
#endif
            dsInfo->seqNum[oldWI] = 0xffff;
            dsInfo->sendIndex = (u16)((dsInfo->sendIndex + 1) % WM_DS_DATASET_NUM);
        }
        else
        {
            if ( res != WM_ERRCODE_SUCCESS && res != WM_ERRCODE_OPERATING )
            {
                WM_WARNING( "WM_SetMPDataToPort failed during data-sharing. errcode=%x\n", res );
                dsInfo->state = WM_DS_STATE_ERROR;
            }
        }
    }
    else
    {
        (void)OS_RestoreInterrupts( enabled );
    }
}

/*---------------------------------------------------------------------------*
  Name:         WM_GetSharedDataAddress

  Description:  Gets specific AID address in data sharing receive data.

  Arguments:    dsInfo      -   WMDataSharingInfo structure
                receiveData -   received shared data
                aid         -   AID

  Returns:      u16*        -   Receive data address. Returns NULL if it doesn't exist.
 *---------------------------------------------------------------------------*/
u16*
WM_GetSharedDataAddress( WMDataSharingInfo* dsInfo, WMDataSet* receiveData, u16 aid )
{
    // parameter check
    if( dsInfo == NULL )
    {
        WM_WARNING( "Parameter \"dsInfo\" must not be NULL.\n" );
        return NULL;
    }
    if( receiveData == NULL )
    {
        WM_WARNING( "Parameter \"receiveData\" must not be NULL.\n" );
        return NULL;
    }

    if ( ! (receiveData->receivedBitmap & ( 1 << aid ) ) )
    {
        // cannot receive data
        return NULL;
    }

    return WmGetSharedDataAddress( dsInfo, receiveData->aidBitmap, receiveData->data, aid );
}

/*---------------------------------------------------------------------------*
  Name:         WmGetSharedDataAddress

  Description:  Gets the specific AID address in the receiving buffer 

  Arguments:    dsInfo - WMDataSharingInfo structure
                aidBitmap - Other communication party included in the data 
                receiveBuf - Receiving buffer 
                aid - AID

  Returns:      u16*    - The address for the receiving data. If there is no data, returns NULL. 
 *---------------------------------------------------------------------------*/
u16*
WmGetSharedDataAddress( WMDataSharingInfo* dsInfo, u16 aidBitmap, u16* receiveBuf, u16 aid )
{
    int iAid;
    u8* data;
    u16 mask;
#ifdef SDK_DEBUG
    u16 count = 0;
#endif
    
    data = (u8*)receiveBuf;
    mask = 0x0001;
    for ( iAid = 0; iAid < aid; iAid++ )
    {
        if ( aidBitmap & mask )
        {
            data += dsInfo->dataLength;
#ifdef SDK_DEBUG
            count+= dsInfo->dataLength;
            if ( count+dsInfo->dataLength > WM_DS_DATA_SIZE )
            {
                OS_Panic( "data pointer overruns. broken dsInfo?" );
            }
#endif
        }
        mask <<= 1;
    }
    
    return (u16*)data;
}

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