FullPulse/app/lib/m_management/sockunion.c

/******************************************************************************
 * file    lib/management/sockunion.c 
 * author  YuLiang
 * version 1.0.0
 * date    14-Sep-2021
 * brief   This file provides all the sockunion operation functions.
 *
 ******************************************************************************
 * Attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2021 LandPower</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of LandPower nor the names of its contributors may be used to 
 *      endorse or promote products derived from this software without specific
 *      prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************/

/* Includes ------------------------------------------------------------------*/
//#ifdef HAVE_CONFIG_H
#include "config.h"
//#endif

#include <fcntl.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <net/if.h>
#include <net/route.h>
#include <arpa/inet.h>

#include "memory.h"
#include "sockunion.h"

/* Private define ------------------------------------------------------------*/

/* Private macro -------------------------------------------------------------*/

/* Private typedef -----------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/
/* Maskbit. */
static const u_char maskbit[] = {0x00, 0x80, 0xc0, 0xe0, 0xf0,
                                 0xf8, 0xfc, 0xfe, 0xff};

/* Static structure for IPv4 access_list's master. */
static access_master_t access_master_ipv4 = 
{ 
    {NULL, NULL},
    {NULL, NULL},
    NULL,
    NULL,
};

#ifdef HAVE_IPV6
/* Static structure for IPv6 access_list's master. */
static access_master_t access_master_ipv6 = 
{ 
    {NULL, NULL},
    {NULL, NULL},
    NULL,
    NULL,
};
#endif /* HAVE_IPV6 */

/* Private function prototypes -----------------------------------------------*/

/* Internal functions --------------------------------------------------------*/
/* Malloc prefix structure. */
static prefix_t *_prefix_new(void)
{
    prefix_t *p = NULL;

    p = XMALLOC(MTYPE_PREFIX, sizeof(prefix_t));
    
    return p;
}

/* Interface functions -------------------------------------------------------*/
/* Free prefix structure. */
void prefix_free(prefix_t *p)
{
    XFREE(MTYPE_PREFIX, p);
}

/* Allocate new prefix_ipv4 structure. */
prefix_ipv4_t *prefix_ipv4_new(void)
{
    prefix_ipv4_t *p = NULL;

    /* Call prefix_new to allocate a full-size struct prefix to avoid problems
     * where the prefix_ipv4_t is cast to struct prefix and unallocated
     * bytes were being referenced (e.g. in structure assignments). */
    p = (prefix_ipv4_t *)_prefix_new();
    p->family = AF_INET;

    return p;
}

#ifdef HAVE_IPV6
/* Allocate a new ip version 6 route */
prefix_ipv6_t *prefix_ipv6_new (void)
{
    prefix_ipv6_t *p = NULL;

    /* Allocate a full-size struct prefix to avoid problems with structure
     * 8 size mismatches. */
    p = (prefix_ipv6_t *)_prefix_new();
    p->family = AF_INET6;
    return p;
}
#endif

static access_master_t *_access_master_get(uint16_t afi)
{
    if (AFI_IP == afi)
        return &access_master_ipv4;
#ifdef HAVE_IPV6
    else if (AFI_IP6 == afi)
        return &access_master_ipv6;
#endif /* HAVE_IPV6 */

    return NULL;
}

/* If filter match to the prefix then return 1. */
static int _filter_match_cisco(filter_t *mfilter, prefix_t *p)
{
    filter_cisco_t *filter = NULL;
    struct in_addr mask;
    uint32_t check_addr = 0;
    uint32_t check_mask = 0;

    filter = &mfilter->u.cfilter;
    check_addr = p->u.prefix4.s_addr & ~filter->addr_mask.s_addr;

    if (filter->extended)
    {
        masklen2ip(p->prefixlen, &mask);
        check_mask = mask.s_addr & ~filter->mask_mask.s_addr;

        if (0 == memcmp(&check_addr, &filter->addr.s_addr, 4) &&
            0 == memcmp(&check_mask, &filter->mask.s_addr, 4))
            return 1;
    }
    else if (0 == memcmp(&check_addr, &filter->addr.s_addr, 4))
        return 1;

    return 0;
}

/* If filter match to the prefix then return 1. */
static int _filter_match_zebra(filter_t *mfilter, prefix_t *p)
{
    filter_zebra_t *filter= NULL;

    filter = &mfilter->u.zfilter;

    if (filter->prefix.family == p->family)
    {
        if (filter->exact)
        {
            if (filter->prefix.prefixlen == p->prefixlen)
                return prefix_match(&filter->prefix, p);
            else
                return 0;
        }
        else
            return prefix_match(&filter->prefix, p);
    }
    else
        return 0;
}

/* Lookup access_list from list of access_list by name. */
access_list_t *access_list_lookup(uint16_t afi, const char *name)
{
    access_list_t *access = NULL;
    access_master_t *master = NULL;

    if (NULL == name)
        return NULL;

    master = _access_master_get(afi);
    if (NULL== master)
        return NULL;

    for(access = master->num.head; access; access = access->next)
    if (0 == strcmp(access->name, name))
            return access;

    for(access = master->str.head; access; access = access->next)
    if (0 == strcmp(access->name, name))
        return access;

    return NULL;
}

/* Apply access list to object (which should be prefix_t *). */
FILTER_TYPE_E access_list_apply(access_list_t *access, void *object)
{
    filter_t *filter = NULL;
    prefix_t *p = NULL;

    p = (prefix_t *)object;

    if (NULL == access)
        return FILTER_DENY;

    for (filter = access->head; filter; filter = filter->next)
    {
        if (filter->cisco)
        {
            if (_filter_match_cisco(filter, p))
                return filter->type;
        }
        else
        {
            if (_filter_match_zebra(filter, p))
                return filter->type;
        }
    }

    return FILTER_DENY;
}

/* Convert masklen into IP address's netmask. */
void masklen2ip(int masklen, struct in_addr *netmask)
{
    uint8_t *pnt = NULL;
    int32_t bit = 0;
    int32_t offset = 0;

    memset(netmask, 0, sizeof(struct in_addr));
    pnt = (unsigned char *)netmask;

    offset = masklen / 8;
    bit = masklen % 8;
  
    while(offset--)
        *pnt++ = 0xff;

    if (bit)
        *pnt = maskbit[bit];
}

/* If n includes p prefix then return 1 else return 0. */
int prefix_match(const prefix_t *n, const prefix_t *p)
{
    int32_t offset = 0;
    int32_t shift = 0;
    const uint8_t *np = NULL;
    const uint8_t *pp = NULL;

    /* If n's prefix is longer than p's one return 0. */
    if (n->prefixlen > p->prefixlen)
        return 0;

    /* Set both prefix's head pointer. */
    np = (const u_char *)&n->u.prefix;
    pp = (const u_char *)&p->u.prefix;

    offset = n->prefixlen / PNBBY;
    shift =  n->prefixlen % PNBBY;

    if (shift)
    if (maskbit[shift] & (np[offset] ^ pp[offset]))
            return 0;

    while (offset--)
    if (np[offset] != pp[offset])
        return 0;
    
    return 1;
}

/* Convert IPv4 compatible IPv6 address to IPv4 address. */
static void _sockunion_normalise_mapped(SOCKUNION_U *su)
{
#ifdef HAVE_IPV6
    struct sockaddr_in sin;

    if (AF_INET6 == su->sa.sa_family &&
        IN6_IS_ADDR_V4MAPPED(&su->sin6.sin6_addr))
    {
        memset(&sin, 0, sizeof(struct sockaddr_in));
        sin.sin_family = AF_INET;
        sin.sin_port = su->sin6.sin6_port;
        memcpy(&sin.sin_addr, ((char *)&su->sin6.sin6_addr) + 12, 4);
        memcpy(su, &sin, sizeof(struct sockaddr_in));
    }
#endif /* HAVE_IPV6 */
}

int str2sockunion(const char *str, SOCKUNION_U *su)
{
    int ret = 0;

    memset(su, 0, sizeof(SOCKUNION_U));

    ret = inet_pton(AF_INET, str, &su->sin.sin_addr);
    /* Valid IPv4 address format. */
    if (ret > 0)
    {
        su->sin.sin_family = AF_INET;
#ifdef HAVE_STRUCT_SOCKADDR_IN_SIN_LEN
        su->sin.sin_len = sizeof(struct sockaddr_in);
#endif
        return 0;
    }
    
#ifdef HAVE_IPV6
    ret = inet_pton(AF_INET6, str, &su->sin6.sin6_addr);
    /* Valid IPv6 address format. */
    if (ret > 0)
    {
        su->sin6.sin6_family = AF_INET6;
#ifdef SIN6_LEN
        su->sin6.sin6_len = sizeof(struct sockaddr_in6);
#endif
        return 0;
    }
#endif

    return -1;
}

const char *sockunion2str(SOCKUNION_U *su, char *buf, size_t len)
{
    if (AF_INET == su->sa.sa_family)
        return inet_ntop(AF_INET, &su->sin.sin_addr, buf, len);    
#ifdef HAVE_IPV6
    else if(AF_INET6 == su->sa.sa_family)
        return inet_ntop(AF_INET6, &su->sin6.sin6_addr, buf, len);
#endif

    return NULL;
}

/* Return accepted new socket file descriptor. */
int sockunion_accept(int sock, SOCKUNION_U *su)
{
    socklen_t len;
    int client_sock;

    len = sizeof(SOCKUNION_U);
    client_sock = accept(sock, (struct sockaddr*)su, &len);
  
    _sockunion_normalise_mapped(su);
    return client_sock;
}

int set_nonblocking(int fd)
{
    int flags = 0;

    /* According to the Single UNIX Spec, the return value for F_GETFL should
     * never be negative. */
    if ((flags = fcntl(fd, F_GETFL)) < 0)
    {
        printh("fcntl(F_GETFL) failed for fd %d: %s", fd, safe_strerror(errno));
        return -1;
    }
    if (fcntl(fd, F_SETFL, (flags | O_NONBLOCK)) < 0)
    {
        printh("fcntl failed setting fd %d non-blocking: %s", fd, safe_strerror(errno));
        return -1;
    }

    return 0;
}

/* Utility function of convert between struct prefix <=> union sockunion. */
prefix_t *sockunion2hostprefix(const SOCKUNION_U *su)
{
    if (su->sa.sa_family == AF_INET)
    {
        prefix_ipv4_t *p = NULL;

        p = prefix_ipv4_new();
        p->family = AF_INET;
        p->prefix = su->sin.sin_addr;
        p->prefixlen = IPV4_MAX_BITLEN;
        return (prefix_t*)p;
    }
    
#ifdef HAVE_IPV6
    if (su->sa.sa_family == AF_INET6)
    {
        prefix_ipv6_t *p = NULL;

        p = prefix_ipv6_new();
        p->family = AF_INET6;
        p->prefixlen = IPV6_MAX_BITLEN;
        memcpy(&p->prefix, &su->sin6.sin6_addr, sizeof(struct in6_addr));
        return (prefix_t*)p;
    }
#endif /* HAVE_IPV6 */

    return NULL;
}

char *sockunion_su2str(SOCKUNION_U *su)
{
    char str[SU_ADDRSTRLEN] = {0};

    switch (su->sa.sa_family)
    {
    case AF_INET:
        inet_ntop(AF_INET, &su->sin.sin_addr, str, sizeof(str));
        break;
#ifdef HAVE_IPV6
    case AF_INET6:
        inet_ntop(AF_INET6, &su->sin6.sin6_addr, str, sizeof(str));
        break;
#endif /* HAVE_IPV6 */
    }

    return XSTRDUP(MTYPE_PREFIX, str);
}

/* Make socket from sockunion union. */
int sockunion_stream_socket(SOCKUNION_U *su)
{
    int32_t sock = 0;

    if (0 == su->sa.sa_family)
        su->sa.sa_family = AF_INET_UNION;

    sock = socket(su->sa.sa_family, SOCK_STREAM, 0);
    if (sock < 0)
        printh("can't make socket sockunion_stream_socket");

    return sock;
}

int sockunion_reuseaddr(int sock)
{
    int ret;
    int on = 1;

    ret = setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (void *)&on, sizeof(on));
    if (ret < 0)
    {
        printh("can't set sockopt SO_REUSEADDR to socket %d", sock);
        return -1;
    }
    
    return 0;
}

/* Bind socket to specified address. */
int sockunion_bind(int sock, SOCKUNION_U *su, unsigned short port, SOCKUNION_U *su_addr)
{
    int size = 0;
    int ret = 0;

    if (AF_INET == su->sa.sa_family)
    {
        size = sizeof(struct sockaddr_in);
        su->sin.sin_port = htons(port);
        if (NULL == su_addr)
            su->sin.sin_addr.s_addr = htonl(INADDR_ANY);
    }
#ifdef HAVE_IPV6
    else if(AF_INET6 == su->sa.sa_family)
    {
        size = sizeof(struct sockaddr_in6);
        su->sin6.sin6_port = htons(port);
        if (NULL == su_addr)
            memset (&su->sin6.sin6_addr, 0, sizeof(struct in6_addr));
    }
#endif /* HAVE_IPV6 */
  

    ret = bind(sock, (struct sockaddr *)su, size);
    if (ret < 0)
        printh("can't bind socket : %s\n", safe_strerror(errno));

    return ret;
}

int sockunion_ip_set(char *name, unsigned int addr)
{
    int fd = 0;
    struct ifreq ifr;
    struct sockaddr_in *sin = NULL;

    fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (fd < 0)
    {
        printh("Ip set socket error!\r\n");
        return -1;
    }
    
    memset(&ifr, 0, sizeof(ifr));
    strcpy(ifr.ifr_name, name);
    sin = (struct sockaddr_in*)&ifr.ifr_addr;
    sin->sin_family = AF_INET;
    
    /* IP地址 */
    sin->sin_addr.s_addr = addr;
    if (ioctl(fd, SIOCSIFADDR, &ifr) < 0)
    {
        close(fd);
        printh("Ip set ioctl SIOCSIFADDR error!\r\n");
        return -2;
    }

    close(fd);
    return 0;
}

int sockunion_mask_set(char *name, unsigned int mask)
{
    int fd = 0;
    struct ifreq ifr;
    struct sockaddr_in *sin = NULL;

    fd = socket(AF_INET, SOCK_DGRAM, 0);
    if (fd < 0)
    {
        printh("socket error\r\n");
        return -1;
    }
    
    memset(&ifr, 0, sizeof(ifr));
    strcpy(ifr.ifr_name, name);
    sin = (struct sockaddr_in*)&ifr.ifr_addr;
    sin->sin_family = AF_INET;
    
    /* 子网掩码 */
    sin->sin_addr.s_addr = mask;
    if (ioctl(fd, SIOCSIFNETMASK, &ifr) < 0)
    {
        close(fd);
        printh("ioctl\r\n");
        return -3;
    }

    close(fd);
    return 0;
}

int sockunion_gw_set(char *name, unsigned int gateway, unsigned int gateway_old)
{
    int fd = 0;
    struct rtentry rt;
    int rv = 0;

    fd = socket(PF_INET, SOCK_DGRAM, 0);
    if (fd < 0)
    {
        printh("Gateway set socket error!\r\n");
        return -1;
    }

    /* Delete existing defalt gateway */
    memset(&rt, 0, sizeof(rt));
    rt.rt_dst.sa_family = AF_INET;
    ((struct sockaddr_in *)&rt.rt_dst)->sin_addr.s_addr = 0;
    rt.rt_gateway.sa_family = AF_INET;    
    ((struct sockaddr_in *)&rt.rt_gateway)->sin_addr.s_addr = gateway_old;    
    rt.rt_genmask.sa_family = AF_INET;
    ((struct sockaddr_in *)&rt.rt_genmask)->sin_addr.s_addr = 0;
    rt.rt_flags = RTF_UP;
    rv = ioctl(fd, SIOCDELRT, &rt);

    /* Set default gateway */
    if ((rv == 0 || errno == ESRCH) && gateway)
    {
        memset(&rt, 0, sizeof(rt));
        rt.rt_dst.sa_family = AF_INET;
        ((struct sockaddr_in *)&rt.rt_dst)->sin_addr.s_addr = 0;
        rt.rt_gateway.sa_family = AF_INET;
        ((struct sockaddr_in *)&rt.rt_gateway)->sin_addr.s_addr = gateway;
        rt.rt_genmask.sa_family = AF_INET;
        ((struct sockaddr_in *)&rt.rt_genmask)->sin_addr.s_addr = 0;
        rt.rt_flags = RTF_UP | RTF_GATEWAY;
        rv = ioctl(fd, SIOCADDRT, &rt);
    }

    close(fd);
    return rv;
}  
/************************ (C) COPYRIGHT LandPower ***** END OF FILE ****************/