/*
 * grdxt.c - Three-levels Generic Radix-tree implementation
 * 
 * authors  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_kernel_types.h>
#include <hal_special.h>
#include <errno.h>
#include <printk.h>
#include <vseg.h>
#include <kmem.h>
#include <grdxt.h>

/////////////////////////////////
error_t grdxt_init( grdxt_t * rt,
                    uint32_t  ix1_width,
                    uint32_t  ix2_width,
                    uint32_t  ix3_width )
{
    void      ** root;
	kmem_req_t   req;
  
	rt->ix1_width = ix1_width;
	rt->ix2_width = ix2_width;
	rt->ix3_width = ix3_width;

    // allocates first level array
	req.type  = KMEM_GENERIC;
	req.size  = sizeof(void *) << ix1_width;
	req.flags = AF_KERNEL | AF_ZERO;
	root = kmem_alloc( &req );
	if( root == NULL ) return ENOMEM;
  
	rt->root = root;

	return 0;
}

//////////////////////////////////
void grdxt_destroy( grdxt_t * rt )
{
	kmem_req_t req;

    uint32_t   w1 = rt->ix1_width;
    uint32_t   w2 = rt->ix2_width;
    uint32_t   w3 = rt->ix3_width;

    void    ** ptr1 = rt->root;
    void    ** ptr2;
    void    ** ptr3;

	uint32_t   ix1;
	uint32_t   ix2;

	req.type = KMEM_GENERIC;

	for( ix1=0 ; ix1 < (uint32_t)(1 << w1) ; ix1++ )
	{
        ptr2 = ptr1[ix1];

		if( ptr2 == NULL ) continue;

        for( ix2=0 ; ix2 < (uint32_t)(1 << w2) ; ix2++ )
        {
            ptr3 = ptr2[ix2];

		    if( ptr3 == NULL ) continue;

            // release level 3 array 
		    req.ptr  = ptr3;
            req.type = KMEM_GENERIC;
            req.size = sizeof(void *) * (1 << w3);
		    kmem_free( &req );
        }

        // release level 2 array
		req.ptr  = ptr2;
        req.type = KMEM_GENERIC;
        req.size = sizeof(void *) * (1 << w2);
		kmem_free( &req );
    }

    // release level 1 array
	req.ptr  = ptr1;
    req.type = KMEM_GENERIC;
    req.size = sizeof(void *) * (1 << w1);
	kmem_free( &req );

}  // end grdxt_destroy()

///////////////////////////////
void grdxt_print( grdxt_t * rt,
                  char    * name )
{
	uint32_t        ix1;  
	uint32_t        ix2;
	uint32_t        ix3;

    uint32_t        w1 = rt->ix1_width;
    uint32_t        w2 = rt->ix2_width;
    uint32_t        w3 = rt->ix3_width;

    void         ** ptr1 = rt->root;
    void         ** ptr2;
    void         ** ptr3;

    intptr_t        key; 
    intptr_t        value;

	printk("\n***** Generic Radix tree %s : n1 = %d / n2 = %d / n3 = %d\n\n", 
           name, 1<<w1 , 1<<w2 , 1<<w3 );

	for( ix1=0 ; ix1 < (uint32_t)(1<<w1) ; ix1++ )
	{
        ptr2 = ptr1[ix1];
        if( ptr2 == NULL )  continue;
    
        for( ix2=0 ; ix2 < (uint32_t)(1<<w2) ; ix2++ )
        {
            ptr3 = ptr2[ix2];
            if( ptr3 == NULL ) continue;

            for( ix3=0 ; ix3 < (uint32_t)(1<<w3) ; ix3++ )
            {
                value = (intptr_t)ptr3[ix3];
                if( value == 0 )  continue;

                key = (ix1<<(w2+w3)) + (ix2<<w3) + ix3;
                printk(" - key = %x / value = %x\n", key , value );
            }
        }
	}
} // end grdxt_print()

////////////////////////////////////
error_t grdxt_insert( grdxt_t  * rt,
                      uint32_t   key,
                      void     * value )
{
	kmem_req_t      req;

    uint32_t        w1 = rt->ix1_width;
    uint32_t        w2 = rt->ix2_width;
    uint32_t        w3 = rt->ix3_width;

    // Check key
    if( (key >> (w1 + w2 + w3)) != 0 )
    {
        assert( false ,
        "key value %x exceed (%d + %d + %d) bits", key , w1 , w2 , w3 );
    }

    // compute indexes
    uint32_t        ix1 = key >> (w2 + w3);              // index in level 1 array
	uint32_t        ix2 = (key >> w3) & ((1 << w2) -1);  // index in level 2 array
	uint32_t        ix3 = key & ((1 << w3) - 1);         // index in level 3 array

    void         ** ptr1 = rt->root;                     // pointer on level 1 array
	void         ** ptr2;                                // pointer on level 2 array
	void         ** ptr3;                                // pointer on level 3 array

    // If required, we must allocate memory for the selected level 2 array,
    // and atomically update the level 1 array.
	if( ptr1[ix1] == NULL )
	{
        // allocate memory for level 2 array
        req.type = KMEM_GENERIC;
        req.size = sizeof(void *) << w2;
        req.flags = AF_KERNEL | AF_ZERO;
        ptr2 = kmem_alloc( &req );
        if( ptr2 == NULL) return ENOMEM;

        // update level 1 array
        ptr1[ix1] = ptr2;
	}
    else    // get pointer on selected level 2 array.
    {
	    ptr2 = ptr1[ix1];
    }

    // If required, we must allocate memory for the selected level 3 array,
    // and atomically update the level 2 array.
	if( ptr2[ix2] == NULL )
	{
        // allocate memory for level 3 array
        req.type = KMEM_GENERIC;
        req.size = sizeof(void *) << w3;
        req.flags = AF_KERNEL | AF_ZERO;
        ptr3 = kmem_alloc( &req );
        if( ptr3 == NULL) return ENOMEM;

        //  update level 3 array
		ptr2[ix2] = ptr3;
	}
    else    // get pointer on selected level 3 array.
    {
	    ptr3 = ptr2[ix2];
    }

    // selected slot in level 3 array must be empty
	if( ptr3[ix3] != NULL ) return EEXIST;

    // register the value
	ptr3[ix3] = value;
	hal_fence();

	return 0;
}

///////////////////////////////////
void * grdxt_remove( grdxt_t  * rt,
                     uint32_t   key )
{
    uint32_t        w1 = rt->ix1_width;
    uint32_t        w2 = rt->ix2_width;
    uint32_t        w3 = rt->ix3_width;

    // Check key
    if( (key >> (w1 + w2 + w3)) != 0 )
    {
        assert( false ,
        "key value %x exceed (%d + %d + %d) bits", key , w1 , w2 , w3 );
    }

    // compute indexes
    uint32_t        ix1 = key >> (w2 + w3);              // index in level 1 array
	uint32_t        ix2 = (key >> w3) & ((1 << w2) -1);  // index in level 2 array
	uint32_t        ix3 = key & ((1 << w3) - 1);         // index in level 3 array

    void         ** ptr1 = rt->root;                     // pointer on level 1 array
	void         ** ptr2;                                // pointer on level 2 array
	void         ** ptr3;                                // pointer on level 3 array

    // get ptr2
	ptr2 = ptr1[ix1];
	if( ptr2 == NULL ) return NULL;

    // get ptr3
	ptr3 = ptr2[ix2];
	if( ptr3 == NULL ) return NULL;

    // get value
	void * value = ptr3[ix3];

    // reset selected slot
	ptr3[ix3] = NULL;
	hal_fence();

	return value;
}

///////////////////////////////////
void * grdxt_lookup( grdxt_t  * rt,
                     uint32_t   key )
{
    uint32_t        w1 = rt->ix1_width;
    uint32_t        w2 = rt->ix2_width;
    uint32_t        w3 = rt->ix3_width;

    // Check key
    if( (key >> (w1 + w2 + w3)) != 0 )
    {
        assert( false ,
        "key value %x exceed (%d + %d + %d) bits", key , w1 , w2 , w3 );
    }

    void         ** ptr1 = rt->root;
    void         ** ptr2;
    void         ** ptr3;

    // compute indexes
    uint32_t        ix1 = key >> (w2 + w3);              // index in level 1 array
	uint32_t        ix2 = (key >> w3) & ((1 << w2) -1);  // index in level 2 array
	uint32_t        ix3 = key & ((1 << w3) - 1);         // index in level 3 array

    // get ptr2
	ptr2 = ptr1[ix1];
	if( ptr2 == NULL ) return NULL;

    // get ptr3
	ptr3 = ptr2[ix2];
	if( ptr3 == NULL ) return NULL;

    // get value
	void * value = ptr3[ix3];

	return value;
}

//////////////////////////////////////
void * grdxt_get_first( grdxt_t  * rt,
                        uint32_t   start_key,
                        uint32_t * found_key )
{
    uint32_t        ix1;
    uint32_t        ix2;
    uint32_t        ix3;

    uint32_t        w1 = rt->ix1_width;
    uint32_t        w2 = rt->ix2_width;
    uint32_t        w3 = rt->ix3_width;

    // Check start_key
    if( (start_key >> (w1 + w2 + w3)) != 0 )
    {
        assert( false ,
        "start_key value %x exceed (%d + %d + %d) bits", start_key , w1 , w2 , w3 );
    }

    // compute max indexes
    uint32_t        max1 = 1 << w1;
    uint32_t        max2 = 1 << w2;
    uint32_t        max3 = 1 << w3;

    // compute min indexes
    uint32_t        min1 = start_key >> (w2 + w3);            
	uint32_t        min2 = (start_key >> w3) & ((1 << w2) -1);
	uint32_t        min3 = start_key & ((1 << w3) - 1);  

    void         ** ptr1 = rt->root;
    void         ** ptr2;
    void         ** ptr3;

    for( ix1 = min1 ; ix1 < max1 ; ix1++ )
    {
        ptr2 = ptr1[ix1];
        if( ptr2 == NULL ) continue;

        for( ix2 = min2 ; ix2 < max2 ; ix2++ )
        {
            ptr3 = ptr2[ix2];
            if( ptr3 == NULL ) continue;

            for( ix3 = min3 ; ix3 < max3 ; ix3++ )
            {
                if( ptr3[ix3] == NULL ) continue;
                else                    
                {
                    *found_key = (ix1 << (w2+w3)) | (ix2 << w1) | ix3;
                    return ptr3[ix3];
                }
            }
        }
    }

    return NULL;
}
