/*
 * hal_remote.c - implementation of Generic Remote Access API for TSAR-MIPS32
 * 
 * Authors : Mohammed Karaoui (2015)
 *           Alain Greiner    (2016)
 *
 * Copyright (c) UPMC Sorbonne Universites
 * 
 * This file is part of ALMOS-MKH..
 *
 * ALMOS-MKH. is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2.0 of the License.
 *
 * ALMOS-MKH. is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with ALMOS-MKH.; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <hal_types.h>
#include <hal_irqmask.h>

////////////////////////////////
void hal_remote_sb( xptr_t   xp,
                    uint8_t  data )
{
    uint32_t save_sr;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

/* TODO improve all hal remote functions to include SR handling in assembly code */
/* as it is done below for hal_remote_sb                                         */

    asm volatile( 
        ".set noreorder              \n"
        "mfc0   $14,    $12          \n"  /* $14 <= CP0_SR      */
        "srl    $13,    $14,    1    \n" 
        "sll    $13,    $13,    1    \n"  /* $13 <= SR masked   */
        "mtc0   $13,    $12          \n"  /* IRQ disabled       */
        

        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT   */   
        "mtc2   %2,     $24          \n"  /* PADDR_EXT <= cxy   */   
        "sb     %0,     0(%1)        \n"  /* *paddr <= value    */
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15   */   

        "mtc0   $14,    $12          \n"  /* SR restored        */

        "sync                        \n"
        ".set reorder                \n"
        : : "r" (data), "r" (ptr), "r" (cxy) : "$13", "$14", "$15" );
}

/////////////////////////////////
void hal_remote_sw( xptr_t    xp,
                    uint32_t  data )
{
    uint32_t save_sr;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT */   
        ".set noreorder              \n"
        "mtc2   %2,     $24          \n"  /* PADDR_EXT <= cxy */   
        "sw     %0,     0(%1)        \n"  /* *paddr <= value  */
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15 */   
        "sync                        \n"
        ".set reorder                \n"
        : : "r" (data), "r" (ptr), "r" (cxy) : "$15" );

    hal_restore_irq( save_sr );

}

//////////////////////////////////
void hal_remote_swd( xptr_t    xp,
                     uint64_t  data )
{
    uint32_t save_sr;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    uint32_t data_lsb = (uint32_t)data;
    uint32_t data_msb = (uint32_t)(data>>32);

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT  */   
        "mtc2   %3,     $24          \n"  /* PADDR_EXT <= cxy  */   
        "sw     %0,     0(%2)        \n"  /* *paddr <= lsb     */
        "sw     %1,     4(%2)        \n"  /* *(paddr+4) <= msb */
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15  */   
        "sync                        \n"
        ".set reorder                \n"
        : : "r" (data_lsb), "r" (data_msb), "r" (ptr), "r" (cxy) : "$15" );

    hal_restore_irq( save_sr );

}

////////////////////////////////////
void hal_remote_spt( xptr_t      xp,
                     void *      pt )
{
    hal_remote_sw ( xp , (uint32_t)pt );
}

////////////////////////////////
uint8_t hal_remote_lb( xptr_t  xp )
{
    uint32_t save_sr;
	char     data;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT   */ 
        "mtc2   %2,     $24          \n"  /* PADDR_EXT <= cxy   */   
        "lb     %0,     0(%1)        \n"  /* data <= *paddr     */
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15   */   
        ".set reorder                \n"
        : "=r" (data) : "r" (ptr), "r" (cxy) : "$15" );

    hal_restore_irq( save_sr );

	return ( data );
}

////////////////////////////////////
uint32_t hal_remote_lw( xptr_t  xp )
{
    uint32_t save_sr;
	uint32_t data;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT   */ 
        "mtc2   %2,     $24          \n"  /* PADDR_EXT <= cxy   */   
        "lw     %0,     0(%1)        \n"  /* data <= *paddr     */
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15   */   
        ".set reorder                \n"
        : "=r" (data) : "r" (ptr), "r" (cxy) : "$15" );

    hal_restore_irq( save_sr );

    return ( data );
}

/////////////////////////////////////
uint64_t hal_remote_lwd( xptr_t  xp )
{
    uint32_t save_sr;
    uint32_t data_lsb;
    uint32_t data_msb;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT     */ 
        "mtc2   %3,     $24          \n"  /* PADDR_EXT <= cxy     */   
        "lw     %0,     0(%2)        \n"  /* data_lsb <= *paddr   */
        "lw     %1,     4(%2)        \n"  /* data_msb <= *paddr+4 */
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15     */   
        ".set reorder                \n"
        : "=r" (data_lsb), "=r"(data_msb) : "r" (ptr), "r" (cxy) : "$15" );

    hal_restore_irq( save_sr );

    return ( (((uint64_t)data_msb)<<32) + (((uint64_t)data_lsb)) );

}

////////////////////////////////////
void * hal_remote_lpt( xptr_t    xp )
{
    return (void *)hal_remote_lw ( xp );
}

///////////////////////////////////////////
bool_t hal_remote_atomic_cas( xptr_t    xp,
                              uint32_t  old,
                              uint32_t  new )
{
    uint32_t save_sr;
	bool_t   isAtomic;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT   */ 
        "mtc2   %4,     $24          \n"  /* PADDR_EXT <= cxy   */   
        "or     $8,     $0,    %3    \n"  /* $8 <= new          */
        "ll     $3,    0(%1)         \n"  /* $3 <= *paddr       */
        "bne    $3,     %2,    1f    \n"  /* if ($3 != old)     */
        "li     $7,     0            \n"  /* $7 <= 0            */
        "sc     $8,     (%1)         \n"  /* *paddr <= new      */
        "or     $7,     $8,    $0    \n"  /* $7 <= atomic       */
        "sync                        \n"
        "1:                          \n"
        "or     %0,     $7,    $0    \n"  /* isAtomic <= $7     */
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15   */   
        ".set reorder                \n"
        : "=&r" (isAtomic) : "r" (ptr), "r" (old) , "r" (new), "r" (cxy) 
		: "$3", "$7", "$8", "$15" );

    hal_restore_irq( save_sr );

	return isAtomic;

}  // end hal_remote_atomic_cas()

////////////////////////////////////////////
uint32_t hal_remote_atomic_add( xptr_t   xp, 
                                uint32_t incr )
{	
    uint32_t save_sr;
	uint32_t current;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT     */ 
        "mtc2   %3,     $24          \n"  /* PADDR_EXT <= cxy     */   
        "1:                          \n"
        "ll     %0,     (%1)         \n"  /* current <= *paddr    */
        "addu   $3,     %0,     %2   \n"  /* $3 <= current + incr */
        "sc     $3,     (%1)         \n"  /* *paddr <= $3         */
        "beq    $3,     $0,     1b   \n"  /* retry if failure     */
        "nop                         \n"
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15     */   
        ".set reorder                \n"
		: "=&r" (current) : "r" (ptr), "r" (incr), "r" (cxy) : "$3", "$15" );

    hal_restore_irq( save_sr );

	return current;

}  // end hal_remote_atomic_add()

////////////////////////////////////////////
uint32_t hal_remote_atomic_and( xptr_t   xp, 
                                uint32_t mask )
{	
    uint32_t save_sr;
	uint32_t current;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT     */ 
        "mtc2   %3,     $24          \n"  /* PADDR_EXT <= cxy     */   
        "1:                          \n"
        "ll     %0,     (%1)         \n"  /* current <= *paddr    */
        "and    $3,     %0,     %2   \n"  /* $3 <= current & mask */
        "sc     $3,     (%1)         \n"  /* *paddr <= $3         */
        "beq    $3,     $0,     1b   \n"  /* retry if failure     */
        "nop                         \n"
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15     */   
        ".set reorder                \n"
	    : "=&r" (current) : "r" (ptr), "r" (mask), "r" (cxy) : "$3", "$15" );

    hal_restore_irq( save_sr );

	return current;

}  // end hal_remote_atomic_and()

////////////////////////////////////////////
uint32_t hal_remote_atomic_or( xptr_t   xp, 
                               uint32_t mask )
{	
    uint32_t save_sr;
	uint32_t current;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT     */ 
        "mtc2   %3,     $24          \n"  /* PADDR_EXT <= cxy     */   
        "1:                          \n"
        "ll     %0,     (%1)         \n"  /* current <= *paddr    */
        "or     $3,     %0,     %2   \n"  /* $3 <= current | mask */
        "sc     $3,     (%1)         \n"  /* *paddr <= $3         */
        "beq    $3,     $0,     1b   \n"  /* retry if failure     */
        "nop                         \n"
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15     */   
        ".set reorder                \n"
	    : "=&r" (current) : "r" (ptr), "r" (mask), "r" (cxy) : "$3", "$15" );

    hal_restore_irq( save_sr );

	return current;

}  // end hal_remote_atomic_or()

/////////////////////////////////////////////////
error_t hal_remote_atomic_try_add( xptr_t     xp,
                                   uint32_t   incr,
                                   uint32_t * old )
{
    uint32_t save_sr;
	uint32_t current;
	error_t  error;
    uint32_t ptr = (uint32_t)GET_PTR( xp );
    uint32_t cxy = (uint32_t)GET_CXY( xp );

    hal_disable_irq( &save_sr );

    asm volatile( 
        ".set noreorder              \n"
        "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT     */ 
        "mtc2   %4,     $24          \n"  /* PADDR_EXT <= cxy     */   
        "ll     %0,     (%2)         \n"  /* current <= *paddr    */
        "addu   $3,     %0,     %3   \n"  /* $3 <= current + incr */
        "sc     $3,     (%2)         \n"  /* *paddr <= $3         */
        "beq    $3,     $0,     1f   \n"  /* exit if failure      */
        "ori    %1,     $0,      1   \n"  /* fail: ret <= 1       */
        "and    %1,     $0,     $0   \n"  /* success: ret <= 0    */
        "1:                          \n"
        "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15     */   
        ".set reorder                \n"
		: "=&r" (current), "=&r" (error) : "r" (ptr), "r" (incr), "r" (cxy) : "$3", "$15" );
		
    hal_restore_irq( save_sr );

    *old = current;
		
	return error;

}  // end hal_remote_atomic_try_add()

/////////////////////////////////////
void hal_remote_memcpy( xptr_t   dst,
                        xptr_t   src,
                        uint32_t size )
{
    uint32_t save_sr;
	uint32_t i;
	uint32_t wsize;
    uint32_t dptr = (uint32_t)GET_PTR( dst );
    uint32_t dcxy = (uint32_t)GET_CXY( dst );
    uint32_t sptr = (uint32_t)GET_PTR( src );
    uint32_t scxy = (uint32_t)GET_CXY( src );

    hal_disable_irq( &save_sr );

	if( (dptr & 0x3) || (sptr & 0x3) ) wsize = 0;  // do it all in bytes
    else                               wsize = size >> 2;

	for( i = 0 ; i < wsize ; i++ )          // transfer one word per iteration
	{
        asm volatile( 
            ".set noreorder              \n"
            "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT     */ 
            "mtc2   %0,     $24          \n"  /* PADDR_EXT <= scxy    */   
		    "lw     $3,     0(%1)	     \n"  /* $3 <= *src           */				
            "mtc2   %2,     $24          \n"  /* PADDR_EXT <= dcxy    */	
            "sw     $3,     0(%3)        \n"  /* *dst <= $3           */
            "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15     */   
            ".set reorder                \n"
		    : : "r"(scxy), "r" (sptr+(i<<2)), "r"(dcxy), "r" (dptr+(i<<2)) : "$3", "$15" );		
	}

	for( i = wsize << 2 ; i < size ; i++ )  // transfer one byte per iteration
	{
        asm volatile( 
            ".set noreorder              \n"
            "mfc2   $15,    $24          \n"  /* $15 <= PADDR_EXT     */ 
            "mtc2   %0,     $24          \n"  /* PADDR_EXT <= scxy    */   
		    "lb 	$3,     0(%1)	     \n"  /* $3 <= *src           */				
            "mtc2   %2,     $24          \n"  /* PADDR_EXT <= dcxy    */	
            "sb     $3,     0(%3)        \n"  /* *dst <= $3           */
            "mtc2   $15,    $24          \n"  /* PADDR_EXT <= $15     */   
            ".set reorder                \n"
		    : : "r"(scxy), "r" (sptr+i), "r"(dcxy), "r" (dptr+i) : "$3", "$15" );		
	}

    hal_restore_irq( save_sr );

}  // end hal_remote_memcpy()

///////////////////////////////////
void hal_remote_strcpy( xptr_t dst,
                        xptr_t src )
{
    uint32_t save_sr;
    uint32_t dptr = (uint32_t)GET_PTR( dst );
    uint32_t dcxy = (uint32_t)GET_CXY( dst );
    uint32_t sptr = (uint32_t)GET_PTR( src );
    uint32_t scxy = (uint32_t)GET_CXY( src );

    hal_disable_irq( &save_sr );

    // loop on characters while non NUL 
    asm volatile(
        "mfc2   $15,   $24          \n"   /* $15 <= PADDR_EXT               */
        "1:                         \n"   /* loop entry                     */
        "mtc2   %0,    $24          \n"   /* PADDR_EXT <= scxy              */   
        "lb	    $13,   0(%1)        \n"   /* read char from src string      */
        "mtc2   %2,    $24          \n"   /* PADDR_EXT <= dcxy              */   
	    "sb	    $13,   0(%3)        \n"   /* store char to dst string       */
        "addi   %1,    %1,  1       \n"   /* increment sptr pointer         */
        "addi   %3,    %3,  1       \n"   /* increment dptr pointer         */
        "bne    $13,   $0,  1b      \n"   /* test NUL                       */
        "nop                        \n"
        "mtc2   $15,   $24          \n"   /* PADDR_EXT <= $15               */   
        : : "r"(scxy), "r"(sptr), "r"(dcxy), "r"(dptr) : "$13","$15", "memory" );

    hal_restore_irq( save_sr );

} // end hal_remote_strcpy()