/*
 ******************************************************************************
 *                     INTERPEAK SOURCE FILE
 *
 *   Document no: @(#) $Name: VXWORKS_ITER40_2019102207 $ $RCSfile: ipmip4_lla.c,v $ $Revision: 1.10 $
 *   $Source: /home/interpeak/CVSRoot/ipmip4/src/ipmip4_lla.c,v $
 *   $Author: jhorteli $
 *   $State: Exp $ $Locker:  $
 *
 *   INTERPEAK_COPYRIGHT_STRING
 *     Design and implementation by Joel Hortelius <joel.hortelis@interpeak.se>
 ******************************************************************************
 */


/*
 ****************************************************************************
 * 1                    DESCRIPTION
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 2                    CONFIGURATION
 ****************************************************************************
 */

#include "ipmip4_config.h"


/*
 ****************************************************************************
 * 3                    INCLUDE FILES
 ****************************************************************************
 */

#define IPCOM_USE_CLIB_PROTO
#include <ipcom_clib.h>
#include <ipcom_drv_eth_filter.h>
#include <ipcom_err.h>
#include <ipcom_hash.h>
#include <ipcom_inet.h>
#include <ipcom_ipd.h>
#include <ipcom_list.h>
#include <ipcom_os.h>
#include <ipcom_sock.h>
#include <ipcom_sock2.h>
#include <ipcom_sysctl.h>
#include <ipcom_syslog.h>
#include <ipcom_netif.h>
#include <ipcom_sysvar.h>

#include <ipmip.h>

#include "ipmip4_h.h"
#include <private/ipmip4_lla.h>
#include <private/ipmip4_util.h>

/*
 ****************************************************************************
 * 4                    DEFINES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 5                    TYPES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 6                    EXTERN PROTOTYPES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 7                    LOCAL PROTOTYPES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 8                    DATA
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 9                    STATIC FUNCTIONS
 ****************************************************************************
 */

/*
 *===========================================================================
 *                    ipmip4_lla_from_lladdr_types
 *===========================================================================
 * Description: 
 * Parameters:  
 * Returns:     
 *
 */
IP_STATIC Ip_u8 
ipmip4_lla_llh_from_lladdr_types(Ip_u8        type,
                                 Ip_u8        subtype)
{
    switch (type)
    {
    case IPMIP_LLADDR_TYPE_3GPP2:
        return IPMIP4_ICMP_LLA_EXT_SUBTYPE_3GPP2_MOBILE_STATION;
    case IPMIP_LLADDR_TYPE_3GPP:
        return IPMIP4_ICMP_LLA_EXT_SUBTYPE_3GPP_MOBILE_SUBSCRIBER;
    case IPMIP_LLADDR_TYPE_MAC:
        return IPMIP4_ICMP_LLA_EXT_SUBTYPE_MAC;
    case IPMIP_LLADDR_TYPE_EUID64:
        return IPMIP4_ICMP_LLA_EXT_SUBTYPE_EUID64;
    case IPMIP_LLADDR_TYPE_INET:
        switch (subtype)
        {
        case IPMIP_LLADDR_SUBTYPE_INET_FA:
            return IPMIP4_ICMP_LLA_EXT_SUBTYPE_FA_ADDRESS;
        case IPMIP_LLADDR_SUBTYPE_INET_SOLICITED:
            return IPMIP4_ICMP_LLA_EXT_SUBTYPE_SOLICITED_ADDRESS;
        }
        break;
    case IPMIP_LLADDR_TYPE_BSSID:
        return IPMIP4_ICMP_LLA_EXT_SUBTYPE_ACCESS_POINT;
    }

    return 0;
}

/*
 *===========================================================================
 *                    ipmip4_lla_to_lladdr_type
 *===========================================================================
 * Description: 
 * Parameters:  
 * Returns:     
 *
 */
IP_STATIC Ip_u8 
ipmip4_lla_llh_to_lladdr_type(Ip_u8       lla_type)
{
    switch (lla_type)
    {
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_3GPP2_MOBILE_STATION:
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_3GPP_MOBILE_SUBSCRIBER:
        return IPMIP_LLADDR_TYPE_3GPP2;
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_MAC:
        return IPMIP_LLADDR_TYPE_MAC;
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_EUID64:
        return IPMIP_LLADDR_TYPE_EUID64;
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_SOLICITED_ADDRESS:
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_FA_ADDRESS:
        return IPMIP_LLADDR_TYPE_INET;
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_ACCESS_POINT:
        return IPMIP_LLADDR_TYPE_BSSID;
    default:
        return IPMIP_LLADDR_TYPE_NONE;
    }
}


/*
 *===========================================================================
 *                    ipmip4_lla_to_lladdr_subtype
 *===========================================================================
 * Description: 
 * Parameters:  
 * Returns:     
 *
 */
IP_STATIC Ip_u8
ipmip4_lla_llh_to_lladdr_subtype(Ip_u8 lla_type)
{
    switch (lla_type)
    {
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_SOLICITED_ADDRESS:
        return IPMIP_LLADDR_SUBTYPE_INET_SOLICITED;
    case IPMIP4_ICMP_LLA_EXT_SUBTYPE_FA_ADDRESS:
        return IPMIP_LLADDR_SUBTYPE_INET_FA;
    default:
        return 0;
    }
    
}

/*
 *===========================================================================
 *                    ipmip4_lla_llh_find_array_lladdr
 *===========================================================================
 * Description: Return a usable source LLA address based on found
 *              extensions and a subtype
 * Parameters:  subtype - The specific subtype
 *              ext     - The found LLA extensions.
 * Returns:     The LLA address if it found; IP_NULL Otherwise.
 *
 */
IP_STATIC Ipmip4_icmp_lla_ext_t *
ipmip4_lla_llh_find_array_lladdr(Ipmip_lladdr_t         *lladdr,
                                 Ipmip4_msgext_array_t  *ext)
{
    if (lladdr)
        return ipmip4_lla_llh_find_array_subtype(ipmip4_lla_llh_from_lladdr_types(lladdr->type, lladdr->subtype), ext);

    return IP_NULL;
}

/*
 ****************************************************************************
 * 10                   GLOBAL FUNCTIONS
 ****************************************************************************
 */


/*
 *===========================================================================
 *                    ipmip4_lla_llh_find_array_subtype
 *===========================================================================
 * Description: Return a usable source LLA address based on found
 *              extensions and a subtype
 * Parameters:  subtype - The specific subtype
 *              ext     - The found LLA extensions.
 * Returns:     The LLA address if it found; IP_NULL Otherwise.
 *
 */
IP_GLOBAL Ipmip4_icmp_lla_ext_t *
ipmip4_lla_llh_find_array_subtype(Ip_u8                  subtype,
                                  Ipmip4_msgext_array_t  *ext)
{
    int i;

    /* Locate a LLA with the same type as the interface */
    for (i = 0; i < ext->found; i++)
    {
        Ipmip4_icmp_lla_ext_t *lla = (Ipmip4_icmp_lla_ext_t *)ext->ext[i];
        if (lla->subtype == subtype)
            return lla;
    }

    /* No match */
    return IP_NULL;
}

/*
 *===========================================================================
 *                    ipmip4_lla_llh_convert_lla_to_lladdr
 *===========================================================================
 * Description: Return a usable source LLA address based on found
 *              extensions and a subtype
 * Parameters:  subtype - The specific subtype
 *              ext     - The found LLA extensions.
 * Returns:     The LLA address if it found; IP_NULL Otherwise.
 *
 */
IP_GLOBAL Ipmip_lladdr_t *
ipmip4_lla_llh_convert_lla_to_lladdr(Ipmip_lladdr_t         *lladdr,
                                     Ipmip4_icmp_lla_ext_t  *lla)
{
    Ipmip_lladdr_t  *nlladdr;

    if (!lla)
        return IP_NULL;

    nlladdr = ipmip_lladdr_build(IP_NULL,
                                 ipmip4_lla_llh_to_lladdr_type(lla->subtype),
                                 ipmip4_lla_llh_to_lladdr_subtype(lla->subtype),
                                 &lla->lla[0],
                                 IPMIP4_ICMP_LLA_EXT_LLA_SIZE(lla));
    if (lladdr && nlladdr)
        ipmip_lladdr_sanitize(lladdr, nlladdr);

    return nlladdr;
}


/*
 *===========================================================================
 *                    ipmip4_lla_llh_convert_array_to_lladdr
 *===========================================================================
 * Description: Return a usable source LLA address based on found
 *              extensions and a subtype
 * Parameters:  subtype - The specific subtype
 *              ext     - The found LLA extensions.
 * Returns:     The LLA address if it found; IP_NULL Otherwise.
 *
 */
IP_GLOBAL Ipmip_lladdr_t *
ipmip4_lla_llh_convert_array_to_lladdr(Ipmip_lladdr_t         *lladdr,
                                       Ipmip4_msgext_array_t  *ext)
{
    Ipmip4_icmp_lla_ext_t *lla = ipmip4_lla_llh_find_array_lladdr(lladdr, ext);

    if (!lla)
        return IP_NULL;

    return ipmip_lladdr_build(IP_NULL,
                              lladdr->type,
                              lladdr->subtype,
                              &lla->lla[0],
                              IPMIP4_ICMP_LLA_EXT_LLA_SIZE(lla));
}


/*
 *===========================================================================
 *                    ipmip4_lla_buffer_icmp_append
 *===========================================================================
 * Description: Append an LLA to a buffer in ICMP mode
 * Parameters:  buffer      - The buffer
 *              lla         - The LLA
 * Returns:     The offset
 *
 */
IP_GLOBAL int 
ipmip4_lla_llh_lladdr_buffer_icmp_append(Ipmip_buffer_t       *buffer,
                                         Ipmip_lladdr_t       *lladdr)
{
    /**/
    int off = ipmip_buffer_append(buffer, lladdr, lladdr->len);
    if (off != -1)
    {
        Ipmip4_icmp_lla_ext_t *lla;
        lla = ipmip_buffer_offset(buffer, off);

        /* Convert */
        lla->length     = lladdr->len - 2;
        lla->type       = IPMIP4_ICMP_EXT_TYPE_LLA;
        lla->subtype    = ipmip4_lla_llh_from_lladdr_types(lladdr->type, lladdr->subtype);
    }   

    return off;
}

/*
 *===========================================================================
 *                    ipmip4_lla_lladdr_buffer_udp_append
 *===========================================================================
 * Description: Append an LLA to a buffer in ICMP mode
 * Parameters:  buffer      - The buffer
 *              lla         - The LLA
 * Returns:     The offset
 *
 */
IP_GLOBAL int 
ipmip4_lla_llh_lladdr_buffer_udp_append(Ipmip_buffer_t       *buffer,
                                        Ipmip_lladdr_t       *lladdr)
{
    /**/
    int off = ipmip_buffer_append(buffer, lladdr, lladdr->len);
    if (off != -1)
    {
        Ipmip4_icmp_lla_ext_t *lla;
        lla = ipmip_buffer_offset(buffer, off);

        /* Convert */
        lla->length     = lladdr->len - 2;
        lla->type       = IPMIP4_EXT_TYPE_LLA;
        lla->subtype    = ipmip4_lla_llh_from_lladdr_types(lladdr->type, lladdr->subtype);
    }   

    return off;
}


/*
 *===========================================================================
 *                    ipmip4_lla_from_lladdr_types
 *===========================================================================
 * Description: 
 * Parameters:  
 * Returns:     
 *
 */
IP_GLOBAL Ip_u8 
ipmip4_lla_pmipv4_access_type_from_lladdr_types(Ip_u8        type,
                                                Ip_u8        subtype)
{
    switch (type)
    {
    case IPMIP_LLADDR_TYPE_3GPP2:
        switch (subtype)
        {            
        case IPMIP_LLADDR_SUBTYPE_3GPP2_1xRTT_HRPD:
            return IPMIP4_PMIPV4_ACCESS_TYPE_3GPP2_1XRTT_HRPD;
        case IPMIP_LLADDR_SUBTYPE_3GPP2_UMB:
        default:
            return IPMIP4_PMIPV4_ACCESS_TYPE_3GPP2_UMB;
        }
    case IPMIP_LLADDR_TYPE_3GPP:
        switch (subtype)
        {            
        case IPMIP_LLADDR_SUBTYPE_3GPP_EUTRAN_LTE:
            return IPMIP4_PMIPV4_ACCESS_TYPE_3GPP_EUTRAN_LTE;
        case IPMIP_LLADDR_SUBTYPE_3GPP_UTRAN_GERAN:
        default:
            return IPMIP4_PMIPV4_ACCESS_TYPE_3GPP_UTRAN_GERAN;
        }
    case IPMIP_LLADDR_TYPE_MAC:
        switch (subtype)
        {
        case IPMIP_LLADDR_SUBTYPE_MAC_802_3:
        case IPMIP_LLADDR_SUBTYPE_MAC_802_1Q:
        default:
            return IPMIP4_PMIPV4_ACCESS_TYPE_802_3;
        case IPMIP_LLADDR_SUBTYPE_MAC_802_11:
            return IPMIP4_PMIPV4_ACCESS_TYPE_802_11ABG;
        case IPMIP_LLADDR_SUBTYPE_MAC_802_16E:
            return IPMIP4_PMIPV4_ACCESS_TYPE_802_16E;
        case IPMIP_LLADDR_SUBTYPE_MAC_802_16M:
            return IPMIP4_PMIPV4_ACCESS_TYPE_802_16M;
        }
    }

    return 0;
}

IP_GLOBAL Ip_u8 
ipmip4_lla_pmipv4_device_id_type_from_lladdr_types(Ip_u8        type,
                                                   Ip_u8        subtype)
{
    IPMIP_UNUSED_PARAMETER(subtype);
    switch (type)
    {
    case IPMIP_LLADDR_TYPE_MAC:
        return IPMIP4_PMIPV4_DEVICE_ID_ETH;
    case IPMIP_LLADDR_TYPE_ESN:
        return IPMIP4_PMIPV4_DEVICE_ID_ESN;
    case IPMIP_LLADDR_TYPE_MEID:
        return IPMIP4_PMIPV4_DEVICE_ID_MEID;
    case IPMIP_LLADDR_TYPE_IMEI:
        return IPMIP4_PMIPV4_DEVICE_ID_IMEI;
    default:
        return 0;
    }
}


IP_GLOBAL void
ipmip4_lla_pmipv4_lladdr_buffer_access_type_append(Ipmip_buffer_t       *buffer,
                                                   Ipmip_lladdr_t       *lladdr)
{
    /* Convert LLADDR to Interface ID */
    Ip_u8 attype = ipmip4_lla_pmipv4_access_type_from_lladdr_types(lladdr->type, lladdr->subtype);

    if (attype != 0)
    {
        unsigned char *at = ipmip_buffer_get(buffer, 4);

        if (at != IP_NULL)
        {
            at[0] = IPMIP4_EXT_TYPE_PMIPV4;
            at[1] = 2;
            at[2] = IPMIP4_PMIPV4_SUBTYPE_ACCESS_TYPE;
            at[3] = attype;
        }
    }
}

IP_GLOBAL void
ipmip4_lla_pmipv4_lladdr_buffer_interface_id_append(Ipmip_buffer_t       *buffer,
                                                    Ipmip_lladdr_t       *lladdr)
{
    Ip_size_t llen = IPMIP_LLADDR_LEN_LLA(lladdr);
    if (llen > 0)
    {        
        unsigned char *iid = ipmip_buffer_get(buffer, llen + 3);
        if (iid != 0)
        {
            iid[0] = IPMIP4_EXT_TYPE_PMIPV4;
            iid[1] = llen + 1;
            iid[2] = IPMIP4_PMIPV4_SUBTYPE_INTERFACE_ID;
            ipcom_memcpy(&iid[3], IPMIP_LLADDR_DATA(lladdr), llen);
        }
    }
}

IP_GLOBAL void
ipmip4_lla_pmipv4_lladdr_buffer_device_id_append(Ipmip_buffer_t       *buffer,
                                                 Ipmip_lladdr_t       *lladdr)
{
    Ip_size_t llen = IPMIP_LLADDR_LEN_LLA(lladdr);
    if (llen > 0)
    {        
        Ip_u8 type;
        unsigned char *iid;

        if ((type = ipmip4_lla_pmipv4_device_id_type_from_lladdr_types(lladdr->type, lladdr->subtype)) == 0)
            return;

        iid = ipmip_buffer_get(buffer, llen + 4);
        if (iid != 0)
        {
            iid[0] = IPMIP4_EXT_TYPE_PMIPV4;
            iid[1] = llen + 2;
            iid[2] = IPMIP4_PMIPV4_SUBTYPE_DEVICE_ID;
            iid[3] = type;

            ipcom_memcpy(&iid[4], IPMIP_LLADDR_DATA(lladdr), llen);
        }
    }
}


#if 0
/*
 *===========================================================================
 *                    ipmip4_lla_to_lladdr_type
 *===========================================================================
 * Description: 
 * Parameters:  
 * Returns:     
 *
 */
IP_STATIC Ip_u8 
ipmip4_lla_pmipv4_access_type_to_lladdr_type(Ip_u8       pmipv4_type)
{
    switch (pmipv4_type)
    {
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_3:
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_11ABG:
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_16E:
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_16M:
        return IPMIP_LLADDR_TYPE_MAC;
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP_EUTRAN_LTE:
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP_UTRAN_GERAN:
        return IPMIP_LLADDR_TYPE_3GPP;
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP2_1XRTT_HRPD:
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP2_UMB:
        return IPMIP_LLADDR_TYPE_3GPP2;
    default:
        return IPMIP_LLADDR_TYPE_NONE;
    }
}

IP_STATIC Ip_u8 
ipmip4_lla_pmipv4_access_type_to_lladdr_subtype(Ip_u8       pmipv4_type)
{
    switch (pmipv4_type)
    {
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_3:
        return IPMIP_LLADDR_SUBTYPE_MAC_802_3;
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_11ABG:
        return IPMIP_LLADDR_SUBTYPE_MAC_802_11;
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_16E:
        return IPMIP_LLADDR_SUBTYPE_MAC_802_16E;
    case IPMIP4_PMIPV4_ACCESS_TYPE_802_16M:
        return IPMIP_LLADDR_SUBTYPE_MAC_802_16M;
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP_EUTRAN_LTE:
        return IPMIP_LLADDR_SUBTYPE_3GPP_EUTRAN_LTE;
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP_UTRAN_GERAN:
        return IPMIP_LLADDR_SUBTYPE_3GPP_UTRAN_GERAN;
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP2_1XRTT_HRPD:
        return IPMIP_LLADDR_SUBTYPE_3GPP2_1xRTT_HRPD;
    case IPMIP4_PMIPV4_ACCESS_TYPE_3GPP2_UMB:
        return IPMIP_LLADDR_SUBTYPE_3GPP2_UMB;
    default:
        return IPMIP_LLADDR_TYPE_NONE;
    }
}
#endif

IP_GLOBAL Ipmip_lladdr_t *
ipmip4_lla_pmipv4_interface_id_convert_to_lladdr(Ipmip_lladdr_t         *lladdr,
                                                 Ipmip4_msgext_uhdr_t   *ifid)
{
    Ipmip_lladdr_t                  *nlladdr;
    Ipmip4_pmipv4_interface_id_t    *id = (Ipmip4_pmipv4_interface_id_t *)ifid;    

    nlladdr = ipmip_lladdr_build(IP_NULL,
                                 lladdr->type,
                                 lladdr->subtype,
                                 &id->identifier[0],
                                 IPMIP4_MSGEXT_SHDR_EXT_SIZE(ifid) - 1);
    return nlladdr;
}

IP_GLOBAL Ipmip_lladdr_t *
ipmip4_lla_pmipv4_device_id_convert_to_lladdr(Ipmip_lladdr_t         *lladdr,
                                              Ipmip4_msgext_uhdr_t   *ifid)
{
    Ipmip_lladdr_t                  *nlladdr;
    Ipmip4_pmipv4_device_id_t       *id = (Ipmip4_pmipv4_device_id_t *)ifid;    

    /**/
    nlladdr = ipmip_lladdr_build(IP_NULL,
                                 lladdr->type,
                                 lladdr->subtype,
                                 &id->identifier[0],
                                 IPMIP4_MSGEXT_SHDR_EXT_SIZE(ifid) - 2);
                                 
    return nlladdr;
}

/*
 ****************************************************************************
 * 11                   PUBLIC FUNCTIONS
 ****************************************************************************
 */

/*
 ****************************************************************************
 *                      END OF FILE
 ****************************************************************************
 */