Index: trunk/hal/tsar_mips32/drivers/soclib_pic.c
===================================================================
--- trunk/hal/tsar_mips32/drivers/soclib_pic.c	(revision 141)
+++ trunk/hal/tsar_mips32/drivers/soclib_pic.c	(revision 188)
@@ -2,5 +2,5 @@
  * soclib_pic.c - soclib PIC driver implementation.
  *
- * Author  Alain Greiner (2016)
+ * Author  Alain Greiner (2016,2017)
  *
  * Copyright (c) UPMC Sorbonne Universites
@@ -25,88 +25,457 @@
 #include <chdev.h>
 #include <soclib_pic.h>
-#include <soclib_xcu.h>
 #include <errno.h>
 #include <string.h>
 #include <vfs.h>
+#include <cluster.h>
+#include <printk.h>
+#include <core.h>
+#include <thread.h>
+
+//////////////////////////////////////////////////////////////////////////////////////
+//         Extern variables
+//////////////////////////////////////////////////////////////////////////////////////
+
+extern  chdev_directory_t chdev_dir;    // defined in chdev.h / allocated in kerneL-init.c
+
+extern  iopic_input_t  iopic_input;  // defined in dev_pic.h / allocated in kernel_init.c
+extern  lapic_input_t  lapic_input;  // defined in dev_pic.h / allocated in kernel_init.c
+ 
+//////////////////////////////////////////////////////////////////////////////////////
+//        SOCLIB PIC private functions 
+//////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////
+uint32_t soclib_pic_wti_alloc()
+{
+    uint32_t index;
+
+    // get pointer on cluster extension for SOCLIB PIC (XCU descriptor)
+    soclib_pic_cluster_t * ext_ptr = LOCAL_CLUSTER->pic_extend;
+
+    assert( (ext_ptr->first_free_wti < ext_ptr->wti_nr) , __FUNCTION__ ,
+            "no free WTI found : too much external IRQs\n");
+
+    // update WTI allocator
+    index = ext_ptr->first_free_wti;
+    ext_ptr->first_free_wti++;
+
+    return index;
+
+}  // end soclib_pic_wti_alloc()
+
+///////////////////////////////////////
+inline uint32_t * soclib_pic_xcu_base()
+{
+    return ((soclib_pic_cluster_t *)LOCAL_CLUSTER->pic_extend)->xcu_base;
+
+}  // end soclib_pic_xcu_base()
+
+
+//////////////////////////////////////////
+uint32_t soclib_pic_ack_timer( lid_t lid )
+{
+    // get local XCU segment base 
+	uint32_t * base = soclib_pic_xcu_base();
+
+    // read from register
+	return base[(XCU_PTI_ACK << 5) | lid];
+
+}  // end soclib_pic_ack_timer()
+
+///////////////////////////////////////////
+void soclib_pic_xcu_status( lid_t      lid,
+                            uint32_t * hwi_status,
+                            uint32_t * wti_status,
+                            uint32_t * pti_status )
+{
+    // get local XCU segment base 
+	uint32_t * base = soclib_pic_xcu_base();
+
+    // read PRIO register
+	uint32_t prio = base[(XCU_PRIO << 5) | lid];
+
+    *wti_status = (prio & 0x4) ? (((prio >> 24) & 0x1F) + 1) : 0;
+    *hwi_status = (prio & 0x2) ? (((prio >> 16) & 0x1F) + 1) : 0;
+    *pti_status = (prio & 0x1) ? (((prio >>  8) & 0x1F) + 1) : 0;
+
+}
+
+/////////////////////////////
+void soclib_pic_irq_handler()
+{
+    uint32_t   hwi_status;   // HWI index + 1  / no pending HWI if 0
+    uint32_t   wti_status;   // WTI index + 1  / no pending WTI if 0
+    uint32_t   pti_status;   // PTI index + 1  / no pending PTI if 0
+    chdev_t  * src_chdev;    // pointer on source chdev descriptor
+    uint32_t   index;        // IRQ index
+
+    core_t * core = CURRENT_THREAD->core;
+
+    // get XCU status
+    soclib_pic_xcu_status( core->lid,
+                           &hwi_status,
+                           &wti_status,
+                           &pti_status );
+
+    // analyse status and handle up to 3 pending IRQ (one WTI, one HWI, one PTI)
+
+    if( wti_status )          // pending WTI 
+	{
+        index = wti_status - 1;
+
+        if( index < LOCAL_CLUSTER->cores_nr )   // it is an IPI
+        {
+            assert( (index == core->lid) , __FUNCTION__ , "illegal IPI index" );
+
+            // TODO acknowledge WTI [AG]
+
+            // TODO force scheduling [AG]
+        }
+        else                                    // it is an external device
+        {
+            // get pointer on source chdev
+            src_chdev = ((soclib_pic_core_t *)core->pic_extend)->wti_vector[index];
+
+		    if( src_chdev == NULL )        // strange, but not fatal
+		    {
+                printk("\n[WARNING] in %s : no handler for WTI %d on core %d in cluster %x\n",
+                       __FUNCTION__ , index , core->lid , local_cxy );
+
+	            core->spurious_irqs ++;
+
+                // TODO disable this WTI in local XCU [AG]
+            }
+            else                                 // call relevant ISR
+            {
+		        irq_dmsg("\n[INFO] %s received WTI : index = %d for core %d in cluster %d\n",
+                         __FUNCTION__ , index , core->lid , local_cxy );
+
+                // call ISR
+	            src_chdev->isr( src_chdev );
+            }
+        }
+	}
+
+	if( hwi_status )      // pending HWI
+	{
+        index = hwi_status - 1;
+
+        // get pointer on source chdev
+        src_chdev = ((soclib_pic_core_t *)core->pic_extend)->hwi_vector[index];
+
+		if( src_chdev == NULL )        // strange, but not fatal
+		{
+            printk("\n[WARNING] in %s : no handler for HWI %d on core %d in cluster %x\n",
+                   __FUNCTION__ , index , core->lid , local_cxy );
+
+	        core->spurious_irqs ++;
+
+            // TODO disable this HWI in local XCU [AG]
+		}
+        else                    // call relevant ISR
+        {
+		    irq_dmsg("\n[INFO] %s received HWI : index = %d for core %d in cluster %d\n",
+                     __FUNCTION__ , index , core->lid , local_cxy );
+
+            // call ISR
+		    src_chdev->isr( src_chdev );
+        }
+	}
+
+    if( pti_status )      // pending PTI
+	{
+        index = pti_status - 1;
+
+		irq_dmsg("\n[INFO] %s received PTI : index = %d for cpu %d in cluster %d\n",
+                 __FUNCTION__ , index , core->lid , local_cxy );
+
+        assert( (index == core->lid) , __FUNCTION__ , "unconsistent PTI index\n");
+
+        // acknowledge PTI
+        soclib_pic_ack_timer( index );
+
+        // TODO execute all actions related to TICK event
+        core_clock( core );
+	}
+}  // end soclib_pic_irq_handler()
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////
+//         SOCLIC PIC device  generic API
+//////////////////////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////
+void soclib_pic_init( chdev_t * pic )
+{
+    uint32_t    i;      // for loop on IOPIC inputs
+    uint32_t    x;      // for loop on clusters in a row
+    uint32_t    y;      // for loop on clusters in a column inputs
+    uint32_t    lid;    // for loop on cores in a cluster
+
+    // get target architecture parameters
+    cluster_t * cluster = LOCAL_CLUSTER;
+    uint32_t    x_size  = cluster->x_size;
+    uint32_t    y_size  = cluster->y_size;
+    uint32_t    y_width = cluster->y_width;
+    uint32_t    ncores  = cluster->cores_nr;
+
+    // get IOPIC controller cluster and segment base pointer
+    cxy_t      iopic_seg_cxy = (cxy_t)GET_CXY( pic->base );
+    uint32_t * iopic_seg_ptr = (uint32_t *)GET_PTR( pic->base );
+
+    // reset the IOPIC component registers : mask all input IRQs
+    for( i = 0 ; i < CONFIG_MAX_EXTERNAL_IRQS ; i++ )
+    {
+        xptr_t iopic_seg_xp = XPTR( iopic_seg_cxy,
+                                    iopic_seg_ptr + i*IOPIC_SPAN + IOPIC_MASK ); 
+        hal_remote_sw( iopic_seg_xp , 0 ); 
+    }
+    
+    // GET XCU controller segment base 
+    uint32_t * base = soclib_pic_xcu_base();
+
+    // reset the XCU component registers in all clusters:
+    // mask all HWIs, all WTIs, and all PTIs, for all cores   
+    for( x = 0 ; x < x_size ; x++ )
+    {
+        for( y = 0 ; y < y_size ; y++ )
+        {
+            for( lid = 0 ; lid < ncores ; lid++ )
+            {
+                cxy_t cxy = (x<<y_width) + y;
+                xptr_t hwi_mask_xp = XPTR( cxy , base + (XCU_MSK_HWI_DISABLE << 5 | lid) );
+                xptr_t wti_mask_xp = XPTR( cxy , base + (XCU_MSK_WTI_DISABLE << 5 | lid) );
+                xptr_t pti_mask_xp = XPTR( cxy , base + (XCU_MSK_PTI_DISABLE << 5 | lid) );
+                hal_remote_sw( hwi_mask_xp , 0xFFFFFFFF );
+                hal_remote_sw( wti_mask_xp , 0xFFFFFFFF );
+                hal_remote_sw( pti_mask_xp , 0xFFFFFFFF );
+            }
+        }
+    }
+}  // end soclib_pic_init()
+
+//////////////////////////////////////////////////
+void soclib_pic_extend_init( uint32_t * xcu_base )
+{
+    soclib_pic_cluster_t * cluster_ext_ptr;   
+    soclib_pic_core_t    * core_ext_ptr;
+    kmem_req_t             req;
+    uint32_t               lid;
+    uint32_t               idx;
+
+    cluster_t            * cluster = LOCAL_CLUSTER;
+
+    // create core extension for all cores in cluster
+    for( lid = 0 ; lid < cluster->cores_nr ; lid++ )
+    {
+        // allocate memory for core extension
+        req.type     = KMEM_GENERIC;
+        req.size     = sizeof(soclib_pic_core_t);
+        req.flags    = AF_KERNEL;
+        core_ext_ptr = kmem_alloc( &req );
+
+        assert( (core_ext_ptr != NULL) , __FUNCTION__ ,
+                "cannot allocate memory for core extension\n");
+    
+        // reset the HWI / WTI  interrupt vectors
+        for( idx = 0 ; idx < SOCLIB_MAX_HWI ; idx++ ) core_ext_ptr->hwi_vector[idx] = NULL;
+        for( idx = 0 ; idx < SOCLIB_MAX_WTI ; idx++ ) core_ext_ptr->wti_vector[idx] = NULL;
+
+        // register PIC extension in core descriptor
+        cluster->core_tbl[lid].pic_extend = core_ext_ptr;
+    }
+
+    // allocate memory for cluster extension
+    req.type        = KMEM_GENERIC;
+    req.size        = sizeof(soclib_pic_cluster_t);
+    req.flags       = AF_KERNEL;
+    cluster_ext_ptr = kmem_alloc( &req );
+
+    assert( (cluster_ext_ptr != NULL) , __FUNCTION__ ,
+            "cannot allocate memory for cluster extension\n");
+
+    // get XCU characteristics from the XCU config register 
+    uint32_t  config = xcu_base[XCU_CONFIG<<5];
+    uint32_t  wti_nr = (config >> 16) & 0xFF; 
+    uint32_t  hwi_nr = (config >> 8 ) & 0xFF; 
+    uint32_t  pti_nr = (config      ) & 0xFF; 
+
+    // initialize the cluster extension
+    // The first WTI slots are for IPIs (one slot per core)
+    cluster_ext_ptr->xcu_base       = xcu_base;
+    cluster_ext_ptr->hwi_nr         = hwi_nr;
+    cluster_ext_ptr->wti_nr         = wti_nr;
+    cluster_ext_ptr->pti_nr         = pti_nr;
+    cluster_ext_ptr->first_free_wti = cluster->cores_nr;
+
+    // register PIC extension in cluster manager
+    cluster->pic_extend = cluster_ext_ptr;
+
+}  // end soclib_pic_extend_init()
 
 ////////////////////////////////////////
-void soclib_pic_init( chdev_t  * chdev )
-{
-    // get PIC controller segment cluster and local pointer
-    cxy_t      seg_cxy = (cxy_t)GET_CXY( chdev->base );
-    uint32_t * seg_ptr = (uint32_t *)GET_PTR( chdev->base );
-    uint32_t   i;
-
-    // reset the MASK registers for all input IRQs
-    for( i = 0 ; i < CONFIG_MAX_IRQS_PER_PIC ; i++ )
-    {
-        hal_remote_sw( XPTR( seg_cxy , seg_ptr + i*IOPIC_SPAN + IOPIC_MASK ) , 0 );
-    }
+void soclib_pic_bind_irq( lid_t     lid,
+                          chdev_t * src_chdev )
+{
+    // get extended & local pointers on PIC chdev descriptor
+    xptr_t     pic_xp  = chdev_dir.pic;
+    cxy_t      pic_cxy = GET_CXY( pic_xp );
+    chdev_t *  pic_ptr = (chdev_t *)GET_PTR( pic_xp );
+
+    // get extended and local pointers on IOPIC  segment base
+    xptr_t     seg_pic_xp  = hal_remote_lwd( XPTR( pic_cxy , &pic_ptr->base ) );
+    cxy_t      seg_pic_cxy = GET_CXY( seg_pic_xp );
+    uint32_t * seg_pic_ptr = (uint32_t *)GET_PTR( seg_pic_xp );
+
+    // get local pointer on XCU segment base
+    uint32_t * seg_xcu_ptr = soclib_pic_xcu_base();
+
+    // get the source chdev functionnal type, channel, and direction
+    uint32_t func    = src_chdev->func;
+    uint32_t channel = src_chdev->channel;
+    bool_t   is_rx   = src_chdev->is_rx;
+
+    if( (func == DEV_FUNC_IOC) || (func == DEV_FUNC_NIC) ||
+        (func == DEV_FUNC_TXT) || (func == DEV_FUNC_IOB) )          // external IRQ => WTI
+    {
+        // get external IRQ index
+        uint32_t  irq_id;   
+        if     (  func == DEV_FUNC_IOC            ) irq_id = iopic_input.ioc[channel];
+        else if(  func == DEV_FUNC_TXT            ) irq_id = iopic_input.txt[channel];
+        else if( (func == DEV_FUNC_NIC) &&  is_rx ) irq_id = iopic_input.nic_rx[channel];
+        else if( (func == DEV_FUNC_NIC) && !is_rx ) irq_id = iopic_input.nic_tx[channel];
+        else if(  func == DEV_FUNC_IOB            ) irq_id = iopic_input.iob;
+        else      assert( false , __FUNCTION__ , "illegal device functionnal type\n");
+
+        // get a WTI mailbox from local XCU descriptor  
+        uint32_t wti_id = soclib_pic_wti_alloc();
+
+        // register IRQ type and index in chdev
+        src_chdev->irq_type = SOCLIB_TYPE_WTI;
+        src_chdev->irq_id   = wti_id;
+
+        // compute extended pointer on WTI mailbox in local XCU
+        xptr_t wti_xp = XPTR( local_cxy , &seg_xcu_ptr[(XCU_WTI_REG << 5) | wti_id] );
+
+	    // set the IOPIC_ADDRESS and IOPIC_EXTEND registers in IOPIC
+        uint32_t lsb_wdata = (uint32_t)wti_xp;
+        uint32_t msb_wdata = (uint32_t)(wti_xp >> 32);
+        xptr_t   lsb_xp = XPTR( seg_pic_cxy , seg_pic_ptr+irq_id*IOPIC_SPAN+IOPIC_ADDRESS );
+        xptr_t   msb_xp = XPTR( seg_pic_cxy , seg_pic_ptr+irq_id*IOPIC_SPAN+IOPIC_EXTEND );
+        hal_remote_sw( lsb_xp , lsb_wdata );
+        hal_remote_sw( msb_xp , msb_wdata );
+
+        // unmask IRQ in IOPIC
+        hal_remote_sw( XPTR( seg_pic_cxy , seg_pic_ptr+irq_id*IOPIC_SPAN+IOPIC_MASK ), 1 );
+
+        // update the WTI interrupt vector for core[lid]
+        core_t * core = &LOCAL_CLUSTER->core_tbl[lid];
+        ((soclib_pic_core_t *)core->pic_extend)->wti_vector[wti_id] = src_chdev;
+    }
+    else if( (func == DEV_FUNC_DMA) || (func == DEV_FUNC_MMC) )   // internal IRQ => HWI
+    {
+        // get internal IRQ index
+        uint32_t hwi_id;
+        if( func == DEV_FUNC_DMA ) hwi_id = lapic_input.dma[channel];
+        else                       hwi_id = lapic_input.mmc;
+
+        // register IRQ type and index in chdev
+        src_chdev->irq_type = SOCLIB_TYPE_HWI;
+        src_chdev->irq_id   = hwi_id;
+
+        // update the HWI interrupt vector for core[lid]
+        core_t * core = &LOCAL_CLUSTER->core_tbl[lid];
+        ((soclib_pic_core_t *)core->pic_extend)->wti_vector[hwi_id] = src_chdev;
+    }
+    else
+    {
+        assert( false , __FUNCTION__ , "illegal device functionnal type\n" );
+    } 
+}  // end soclib_pic_bind_irq();
+
+//////////////////////////////////////////
+void soclib_pic_enable_irq( lid_t     lid,
+                            chdev_t * src_chdev )
+{
+    // get local pointer on XCU segment base
+    uint32_t * seg_xcu_ptr = soclib_pic_xcu_base();
+
+    // get the source chdev IRQ type and index
+    uint32_t irq_type = src_chdev->irq_type;
+    uint32_t irq_id   = src_chdev->irq_id;
+
+    if( irq_type == SOCLIB_TYPE_HWI )
+    {
+         // enable this HWI in local XCU controller 
+        seg_xcu_ptr[(XCU_MSK_HWI_ENABLE << 5) | lid] = 1 << irq_id;
+    }
+    else if( irq_type == SOCLIB_TYPE_WTI )
+    {
+         // enable this WTI in local XCU controller 
+        seg_xcu_ptr[(XCU_MSK_WTI_ENABLE << 5) | lid] = 1 << irq_id;
+    }
+    else
+    {
+        assert( false , __FUNCTION__ , "illegal IRQ type\n" );
+    }
+} // end soclib_pic_enable_irq()
+
+///////////////////////////////////////////
+void soclib_pic_disable_irq( lid_t     lid,
+                             chdev_t * src_chdev )
+{
+    // get local pointer on XCU segment base
+    uint32_t * seg_xcu_ptr = soclib_pic_xcu_base();
+
+    // get the source chdev IRQ type and index
+    uint32_t irq_type = src_chdev->irq_type;
+    uint32_t irq_id   = src_chdev->irq_id;
+
+    if( irq_type == SOCLIB_TYPE_HWI )
+    {
+         // disable this HWI in local XCU controller 
+        seg_xcu_ptr[(XCU_MSK_HWI_DISABLE << 5) | lid] = 1 << irq_id;
+    }
+    else if( irq_type == SOCLIB_TYPE_WTI )
+    {
+         // disable this WTI in local XCU controller 
+        seg_xcu_ptr[(XCU_MSK_WTI_DISABLE << 5) | lid] = 1 << irq_id;
+    }
+    else
+    {
+        assert( false , __FUNCTION__ , "illegal IRQ type\n" );
+    }
+} // end soclib_pic_enable_irq()
+
+///////////////////////////////////////////////
+void soclib_pic_enable_timer( uint32_t period )
+{
+    // calling core local index
+    lid_t  lid = CURRENT_CORE->lid;
+
+    // get XCU segment base
+    uint32_t * base = soclib_pic_xcu_base();
+
+    // set period value in XCU
+    base[(XCU_PTI_PER << 5) | lid] = period;
+
+    // enable the PTI in local XCU controller 
+    base[(XCU_MSK_PTI_ENABLE << 5) | lid] = 1 << lid;
 }
 
-////////////////////////////////////////////
-void soclib_pic_bind_irq( xptr_t     dev_xp,
-                          uint32_t   irq_id,
-                          xptr_t     xp_wti )
-{
-    // get PIC device descriptor cluster and local pointer
-    cxy_t     dev_cxy = GET_CXY( dev_xp );
-    chdev_t * dev_ptr = (chdev_t *)GET_PTR( dev_xp );
-
-    // get extended pointer on PIC segment base from PIC device descriptor
-    xptr_t seg_xp = (xptr_t)hal_remote_lwd( XPTR( dev_cxy , &dev_ptr->base ) );
-
-    // get PIC controller segment cluster and local pointer
-    cxy_t      seg_cxy = (cxy_t)GET_CXY( seg_xp);
-    uint32_t * seg_ptr = (uint32_t *)GET_PTR( seg_xp );
-
-    uint32_t lsb = (uint32_t)xp_wti;
-    uint32_t msb  = (uint32_t)(xp_wti>>32);
-
-    // set the IOPIC_ADDRESS and IOPIC_EXTEND registers
-    hal_remote_sw( XPTR( seg_cxy , seg_ptr+irq_id*IOPIC_SPAN+IOPIC_ADDRESS ) , lsb );
-    hal_remote_sw( XPTR( seg_cxy , seg_ptr+irq_id*IOPIC_SPAN+IOPIC_EXTEND  ) , msb );
-
-    // set IOPIC_MASK register
-    hal_remote_sw( XPTR( seg_cxy , seg_ptr+irq_id*IOPIC_SPAN+IOPIC_MASK    ) , 1   );
+///////////////////////////////////////
+void soclib_pic_send_ipi( cxy_t    cxy,
+                          lid_t    lid )
+{
+    // get pointer on local XCU segment base
+    uint32_t * base = soclib_pic_xcu_base();
+
+    // write to WTI mailbox[cxy][lid]
+    hal_remote_sw( XPTR( cxy , &base[(XCU_WTI_REG << 5) | lid] ) , 0 );
 }
 
-//////////////////////////////////////////////
-void soclib_pic_unbind_irq( xptr_t     dev_xp,
-                            uint32_t   irq_id )
-{
-    // get PIC device descriptor cluster and local pointer
-    cxy_t     dev_cxy = GET_CXY( dev_xp );
-    chdev_t * dev_ptr = (chdev_t *)GET_PTR( dev_xp );
-
-    // get extended pointer on PIC segment base from PIC device descriptor
-    xptr_t seg_xp = (xptr_t)hal_remote_lwd( XPTR( dev_cxy , &dev_ptr->base ) );
-
-    // get PIC controller segment cluster and local pointer
-    cxy_t      seg_cxy = (cxy_t)GET_CXY( seg_xp);
-    uint32_t * seg_ptr = (uint32_t *)GET_PTR( seg_xp );
-
-    // set IOPIC_MASK register
-    hal_remote_sw( XPTR( seg_cxy , seg_ptr+irq_id*IOPIC_SPAN+IOPIC_MASK    ) , 0   );
-}
-
-//////////////////////////////////////////////
-void soclib_pic_get_status( xptr_t     dev_xp,
-                            uint32_t   irq_id,
-                            uint32_t * status)
-{
-    // get PIC device descriptor cluster and local pointer
-    cxy_t     dev_cxy = GET_CXY( dev_xp );
-    chdev_t * dev_ptr = (chdev_t *)GET_PTR( dev_xp );
-
-    // get extended pointer on PIC segment base from PIC device descriptor
-    xptr_t seg_xp = (xptr_t)hal_remote_lwd( XPTR( dev_cxy , &dev_ptr->base ) );
-
-    // get PIC controller segment cluster and local pointer
-    cxy_t      seg_cxy = (cxy_t)GET_CXY( seg_xp);
-    uint32_t * seg_ptr = (uint32_t *)GET_PTR( seg_xp );
-
-    // return status
-    *status = hal_remote_lw( XPTR( seg_cxy , seg_ptr+irq_id*IOPIC_SPAN+IOPIC_STATUS ) );
-}
-
+
+
