bcm68620_release/release/host_customized/os_abstraction/posix/bcmos_system.c

/*
<:copyright-BRCM:2016:DUAL/GPL:standard

   Broadcom Proprietary and Confidential.(c) 2016 Broadcom
   All Rights Reserved

Unless you and Broadcom execute a separate written software license
agreement governing use of this software, this software is licensed
to you under the terms of the GNU General Public License version 2
(the "GPL"), available at http://www.broadcom.com/licenses/GPLv2.php,
with the following added to such license:

   As a special exception, the copyright holders of this software give
   you permission to link this software with independent modules, and
   to copy and distribute the resulting executable under terms of your
   choice, provided that you also meet, for each linked independent
   module, the terms and conditions of the license of that module.
   An independent module is a module which is not derived from this
   software.  The special exception does not apply to any modifications
   of the software.

Not withstanding the above, under no circumstances may you combine
this software in any way with any other Broadcom software provided
under a license other than the GPL, without Broadcom's express prior
written consent.

:>
 */

#include "bcmos_system.h"
#ifdef BCMOS_MSG_QUEUE_DOMAIN_SOCKET
#include <sys/un.h>
#endif
#include <sys/resource.h>

/* task control blocks */
extern STAILQ_HEAD(task_list, bcmos_task) task_list;

/* global OS lock */
extern bcmos_mutex bcmos_res_lock;

/*
 * Init
 */

#define TIMER_SIG       SIGRTMIN

/* Initialize system library */
bcmos_errno bcmos_sys_init(void)
{
    return BCM_ERR_OK;
}

/* Clean-up system library */
void bcmos_sys_exit(void)
{
}

/*
 * Task management
 */

/*
 * Default task handler
 */

/* Create a new task */
bcmos_errno bcmos_task_create(bcmos_task *task, const bcmos_task_parm *parm)
{
    F_bcmos_task_handler handler;
    pthread_attr_t pthread_attr;
    struct sched_param pthread_sched_param = {};
    int pthread_prio;
    uint32_t stack_size;
    void *data;
    int rc;

    if (!task || !parm)
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "task %p, parm %p\n", task, parm);
    memset(task, 0, sizeof(*task));
    if (parm->handler)
    {
        /* "traditional task */
        handler = parm->handler;
        data = (void *)parm->data;
    }
    else
    {
        /* "integrated" task */
        handler = bcmos_dft_task_handler;
        data = task;

        /* Initialize and lock mutex to wait on */
        rc = bcmos_sem_create(&task->active_sem, 0, task->parm.flags, parm->name);
        if (rc)
        {
            BCMOS_TRACE_ERR("Task %s: can't create active_sem. Error %s (%d)\n",
                parm->name, bcmos_strerror(rc), rc);
            return rc;
        }
    }

    task->parm = *parm;
    /* Copy name to make sure that it is not released - in case it was on the stack */
    if (task->parm.name)
    {
        strncpy(task->name, task->parm.name, sizeof(task->name) - 1);
        task->parm.name = task->name;
    }
    bcmos_fastlock_init(&task->active_lock, 0);
    bcmos_mutex_lock(&bcmos_res_lock);
    STAILQ_INSERT_TAIL(&task_list, task, list);
    bcmos_mutex_unlock(&bcmos_res_lock);
    task->magic = BCMOS_TASK_MAGIC;
    /* pthread priorities are 1..32, 32 being the highest */
    pthread_prio = 32 - (int)parm->priority;
    if (pthread_prio <= 0)
        pthread_prio = 1;
    stack_size = PTHREAD_STACK_MIN;
    if (parm->stack_size && parm->stack_size > PTHREAD_STACK_MIN)
        stack_size = parm->stack_size;
    rc = pthread_attr_init(&pthread_attr);
    pthread_sched_param.sched_priority = pthread_prio;
    rc = rc ? rc : pthread_attr_setinheritsched(&pthread_attr, PTHREAD_EXPLICIT_SCHED);
    rc = rc ? rc : pthread_attr_setschedpolicy(&pthread_attr, SCHED_RR);
    rc = rc ? rc : pthread_attr_setschedparam(&pthread_attr, &pthread_sched_param);
    rc = rc ? rc : pthread_attr_setstacksize(&pthread_attr, stack_size);
    rc = rc ? rc : pthread_create(&task->sys_task.t, &pthread_attr, (void *(*)(void *))handler, data);
    pthread_attr_destroy(&pthread_attr);
    if (rc == EPERM)
    {
        BCMOS_TRACE_INFO("Thread %s: priority %d is ignored. Start as root to honor priorities\n",
            parm->name, (int)parm->priority);
        rc = pthread_create(&task->sys_task.t, NULL, (void *(*)(void *))handler, data);
    }
    if (rc)
    {
        bcmos_mutex_lock(&bcmos_res_lock);
        STAILQ_REMOVE(&task_list, task, bcmos_task, list);
        bcmos_mutex_unlock(&bcmos_res_lock);
        task->magic = 0;
        if (!parm->handler)
        {
            bcmos_sem_destroy(&task->active_sem);
        }
        BCMOS_TRACE_RETURN(BCM_ERR_SYSCALL_ERR, "%s (%d)\n", strerror(rc), rc);
    }
    return BCM_ERR_OK;
}

/* Destroy task */
bcmos_errno bcmos_task_destroy(bcmos_task *task)
{
    void *res;
    int rc_cancel;
    int rc;

    if (task->magic != BCMOS_TASK_MAGIC)
    {
        return BCM_ERR_PARM;
    }
    task->destroy_request = BCMOS_TRUE;
    task->magic = BCMOS_TASK_MAGIC_DESTROYED;
    bcmos_mutex_lock(&bcmos_res_lock);
    STAILQ_REMOVE(&task_list, task, bcmos_task, list);
    bcmos_mutex_unlock(&bcmos_res_lock);
    /* The task may be waiting on semaphore. Kick it */
    if (!task->parm.handler)
    {
        bcmos_sem_post(&task->active_sem);
    }
    rc_cancel = pthread_cancel(task->sys_task.t);
    rc = pthread_join(task->sys_task.t, &res);
    return (rc || ((!rc_cancel) && (res != PTHREAD_CANCELED))) ? BCM_ERR_SYSCALL_ERR : 0;
}

/** Get current task
 * \returns task handle or NULL if not in task context
 */
bcmos_task *bcmos_task_current(void)
{
    pthread_t pt = pthread_self();
    bcmos_task *t, *tmp;

    STAILQ_FOREACH_SAFE(t, &task_list, list, tmp)
    {
        if (pthread_equal(pt, t->sys_task.t))
            break;
    }
    return t;
}

/*
 * Recursive mutex
 */

/* Create recursive mutex */
bcmos_errno bcmos_mutex_create(bcmos_mutex *mutex, uint32_t flags, const char *name)
{
    int err;
    err = pthread_mutexattr_init(&mutex->attr);
    err = err ? err : pthread_mutexattr_settype(&mutex->attr, PTHREAD_MUTEX_RECURSIVE_NP);
    err = err ? err : pthread_mutex_init(&mutex->m, &mutex->attr);
    if (err)
        BCMOS_TRACE_RETURN(BCM_ERR_SYSCALL_ERR, "errno=%s\n", strerror(err));
    return BCM_ERR_OK;
}

/** Destroy mutex
 * \param[in]   mutex   Mutex control block
 */
void bcmos_mutex_destroy(bcmos_mutex *mutex)
{
    pthread_mutex_destroy(&mutex->m);
    pthread_mutexattr_destroy(&mutex->attr);
}

/* calculate absolute time equal to the current time + timeout */
static inline void _bcmos_get_abs_time(struct timespec *ts, uint32_t timeout)
{
    int rc;
    rc = clock_gettime(CLOCK_REALTIME, ts);
    BUG_ON(rc);
    ts->tv_sec += timeout / 1000000;
    ts->tv_nsec += (timeout % 1000000) * 1000;
    if (ts->tv_nsec > 1000000000)
    {
        ts->tv_sec += ts->tv_nsec / 1000000000;
        ts->tv_nsec %= 1000000000;
    }
}

/** Lock mutex
 */
void bcmos_mutex_lock(bcmos_mutex *mutex)
{
    int rc;
    rc = pthread_mutex_lock(&mutex->m);
    BUG_ON(rc);
    return;
}

/** Release mutex
 * \param[in]   mutex   Mutex control block
 */
void bcmos_mutex_unlock(bcmos_mutex *mutex)
{
    int rc;
    rc = pthread_mutex_unlock(&mutex->m);
    if (rc)
        BCMOS_TRACE_ERR("pthread_mutex_unlock()->%d\n", rc);
    BUG_ON(rc);
}

/*
 * Semaphores
 * Most of semaphore functions are inline
 */

/* Create semaphore */
bcmos_errno bcmos_sem_create(bcmos_sem *sem, uint32_t count, uint32_t flags, const char *name)
{
    sem_init(&sem->s, 0, count);
    return BCM_ERR_OK;
}

/* Destroy semaphore */
void bcmos_sem_destroy(bcmos_sem *sem)
{
    sem_destroy(&sem->s);
}

/* Decrement semaphore counter. Wait if the counter is 0 */
bcmos_errno bcmos_sem_wait(bcmos_sem *sem, uint32_t timeout)
{
    int rc;
    struct timespec ts;

    /* Init end time if necessary */
    if (timeout && timeout != BCMOS_WAIT_FOREVER)
    {
        _bcmos_get_abs_time(&ts, timeout);
    }

    do
    {
        if (timeout == BCMOS_WAIT_FOREVER)
        {
            rc = sem_wait(&sem->s);
        }
        else if (timeout)
        {
            rc = sem_timedwait(&sem->s, &ts);
        }
        else
        {
            rc = sem_trywait(&sem->s);
        }
    } while (rc && errno == EINTR);

    if (rc)
    {
        bcmos_errno err;

        rc = errno;
        if (rc == ETIMEDOUT)
        {
            err = BCM_ERR_TIMEOUT;
        }
        else
        {
            err = BCM_ERR_INTERNAL;
            BCMOS_TRACE_ERR("sem_wait()->%d\n", rc);
        }
        return err;
    }

    return BCM_ERR_OK;
}


/*
 * Timers
 */

/** Get current timestamp
 * \returns the current system timestamp (us)
 */
uint32_t bcmos_timestamp(void)
{
    struct timespec tp;
    clock_gettime(CLOCK_MONOTONIC, &tp);
    return (tp.tv_sec * 1000000 + tp.tv_nsec / 1000);
}

/** Get current timestamp - 64 bit
 * \returns the current system timestamp (us)
 */
uint64_t bcmos_timestamp64(void)
{
    struct timespec tp;
    clock_gettime(CLOCK_MONOTONIC, &tp);
    return ((uint64_t)tp.tv_sec * 1000000LL + (uint64_t)tp.tv_nsec / 1000LL);
}

/*
 * Timer handlers
 */



/* For posix we must create timer task, because there is no way
 * to enforce protection between a signal handler and a thread
 */
static bcmos_task _bcmos_timer_task;
static sem_t _bcmos_timer_lock;

static int _bcmos_tmr_task_handler(long data)
{
    bcmos_sys_timer *timer = (bcmos_sys_timer *)data;
    bcmos_task *this_task = bcmos_task_current();

    while(!this_task->destroy_request)
    {
        sem_wait(&_bcmos_timer_lock);
        timer->handler(timer->data);
    }
    this_task->destroyed = BCMOS_TRUE;

    return 0;
}

/* timer signal handler */
static void timer_sig_handler(int sig, siginfo_t *si, void *uc)
{
    BUG_ON(si->si_code != SI_TIMER);
    sem_post(&_bcmos_timer_lock);
}

/* Create timer */
bcmos_errno bcmos_sys_timer_create(bcmos_sys_timer *timer, bcmos_sys_timer_handler handler, void *data)
{
    static bcmos_task_parm tmr_task_parm = {
        .name = "tmr_task",
        .priority = BCMOS_TASK_PRIORITY_0,
        .handler = _bcmos_tmr_task_handler
    };
    bcmos_errno rc;

    struct sigaction sa;
    struct sigevent sev = {};

    if (!timer || !handler)
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "timer %p, handler %p\n", timer, handler);

    timer->handler = handler;
    timer->data = data;

    sem_init(&_bcmos_timer_lock, 0, 0);
    tmr_task_parm.data = (long)timer;
    rc = bcmos_task_create(&_bcmos_timer_task, &tmr_task_parm);
    if (rc)
        BCMOS_TRACE_RETURN(rc, "Can't create timer task\n");

    sa.sa_flags = SA_SIGINFO;
    sa.sa_sigaction = timer_sig_handler;
    sigemptyset(&sa.sa_mask);
    if (sigaction(TIMER_SIG, &sa, NULL) == -1)
        perror("sigaction");
    /* Prevent timer signal from interrupting system calls */
    if (siginterrupt(TIMER_SIG, 0) == -1)
        perror("siginterrupt");

   /* Create librt timer */
   sev.sigev_notify = SIGEV_SIGNAL;
   sev.sigev_signo = TIMER_SIG;
   sev.sigev_value.sival_ptr = timer;
   if (timer_create(CLOCK_REALTIME, &sev, &timer->t) == -1)
       BCMOS_TRACE_RETURN(BCM_ERR_SYSCALL_ERR, "errno %s\n", strerror(errno));

   return BCM_ERR_OK;
}

/* Destroy timer */
void bcmos_sys_timer_destroy(bcmos_sys_timer *timer)
{
    timer_delete(timer->t);
    sem_destroy(&_bcmos_timer_lock);
    bcmos_task_destroy(&_bcmos_timer_task);
}

/* (Re)start timer */
void bcmos_sys_timer_start(bcmos_sys_timer *timer, uint32_t delay)
{
    struct itimerspec its;

    its.it_value.tv_sec = delay / 1000000;
    its.it_value.tv_nsec = (delay % 1000000) * 1000;
    its.it_interval.tv_sec = 0;
    its.it_interval.tv_nsec = 0;
    if (timer_settime(timer->t, 0, &its, NULL) == -1)
    {
        BCMOS_TRACE_ERR("timer_settime errno %s\n", strerror(errno));
        BUG();
    }
}

/* Stop timer if running */
void bcmos_sys_timer_stop(bcmos_sys_timer *timer)
{
    struct itimerspec its;

    BUG_ON(!timer);
    memset(&its, 0, sizeof(its));
    timer_settime(timer->t, 0, &its, NULL);
}


/* Suspend current task for a time */
void bcmos_usleep(uint32_t us)
{
    uint32_t ts = bcmos_timestamp();
    uint32_t tse = ts + us;
    int32_t d = (us > 1000000) ? 1000000 : us;

    do
    {
        usleep(d);
        d = tse - bcmos_timestamp();
        if (d > 1000000) d = 1000000;
    } while (d > 0);
}

/*
 * Memory management
 */

/* Allocate memory from the main heap */
void *bcmos_alloc(uint32_t size)
{
    return malloc(size);
}

/* Release memory to the main pool */
void bcmos_free(void *ptr)
{
    free(ptr);
}

/*
 * Byte memory pool
 */

/* Memory block header */
typedef struct bcmos_byte_memblk
{
    bcmos_byte_pool *pool;      /** pool that owns the block */
    uint32_t size;              /** block size (bytes) including bcmos_byte_memblk header */
#ifdef BCMOS_MEM_DEBUG
    uint32_t magic;             /** magic number */
#define BCMOS_MEM_MAGIC_ALLOC   (('m'<<24) | ('b' << 16) | ('l' << 8) | 'k')
#define BCMOS_MEM_MAGIC_FREE    (('m'<<24) | ('b' << 16) | ('l' << 8) | '~')
#endif
} bcmos_byte_memblk;

/* Create byte memory pool */
bcmos_errno bcmos_byte_pool_create(bcmos_byte_pool *pool, const bcmos_byte_pool_parm *parm)
{
    if (!pool || !parm)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "pool %p, parm %p\n", pool, parm);
    }

    BCM_MEMZERO_STRUCT(pool);
    pool->parm = *parm;
    if (!pool->parm.size)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "size %u\n", parm->size);
    }
#ifdef BCMOS_MEM_DEBUG
    pool->magic = BCMOS_BYTE_POOL_VALID;
#endif
    return BCM_ERR_OK;
}

/* Destroy memory pool */
bcmos_errno bcmos_byte_pool_destroy(bcmos_byte_pool *pool)
{
    if (pool->allocated)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_STATE, "%u bytes of memory are still allocated from the pool %s\n",
            pool->allocated, pool->parm.name);
    }
#ifdef BCMOS_MEM_DEBUG
    pool->magic = BCMOS_BYTE_POOL_DELETED;
#endif
    return BCM_ERR_OK;
}

/* Allocate memory from memory pool */
void *bcmos_byte_pool_alloc(bcmos_byte_pool *pool, uint32_t size)
{
    bcmos_byte_memblk *blk;
    uint32_t byte_size;
    void *ptr;

#ifdef BCMOS_MEM_DEBUG
    BUG_ON(pool->magic != BCMOS_BYTE_POOL_VALID);
#endif

    if (size + pool->allocated > pool->parm.size)
        return NULL;

    byte_size = size + sizeof(bcmos_byte_memblk);
#ifdef BCMOS_MEM_DEBUG
    byte_size += sizeof(uint32_t); /* block suffix */
#endif
    /* ToDo: Maintain LL of allocated blocks */
    blk = (bcmos_byte_memblk *)malloc(byte_size);
    if (!blk)
        return NULL;
    ptr = (void *)(blk + 1);
    blk->size = byte_size;
    pool->allocated += byte_size;
    blk->pool = pool;
#ifdef BCMOS_MEM_DEBUG
    blk->magic = BCMOS_MEM_MAGIC_ALLOC;
    *(uint32_t *)((long)blk + byte_size - sizeof(uint32_t)) = BCMOS_MEM_MAGIC_ALLOC;
#endif

    return ptr;
}

/* Release memory allocated using bcmos_byte_pool_alloc() */
void bcmos_byte_pool_free(void *ptr)
{
    bcmos_byte_memblk *blk;
    bcmos_byte_pool *pool;

    BUG_ON(!ptr);
    blk = (bcmos_byte_memblk *)((long)ptr - sizeof(bcmos_byte_memblk));
    pool = blk->pool;
#ifdef BCMOS_MEM_DEBUG
    BUG_ON(pool->magic != BCMOS_BYTE_POOL_VALID);
    BUG_ON(blk->magic != BCMOS_MEM_MAGIC_ALLOC);
    BUG_ON(*(uint32_t *)((long)blk + blk->size - sizeof(uint32_t)) != BCMOS_MEM_MAGIC_ALLOC);
    blk->magic = BCMOS_MEM_MAGIC_FREE;
#endif
    pool->allocated -= blk->size;
    free(blk);
}

void _bcmos_backtrace(void)
{
    void *array[32];
    size_t size;
    char **strings;
    size_t i;

    size = backtrace(array, sizeof(array)/sizeof(array[0]));
    strings = backtrace_symbols(array, size);

    printf("Obtained %zd stack frames.\n", size);

    for (i = 0; i < size; i++)
        printf("%s\n", strings[i]);

    free(strings);
}

/* stub for int enable */
void bcmos_int_enable(int irq)
{
}

void bcmos_int_disable(int irq)
{
}

/* stub for int connect */
int bcmos_int_connect(int irq, int cpu, int flags,
    int (*isr)(int irq, void *priv), const char *name, void *priv)
{
    return 0;
}

/* Functions common for domain socket and UDP socket - based message queues */
#if defined(BCMOS_MSG_QUEUE_DOMAIN_SOCKET) || defined(BCMOS_MSG_QUEUE_UDP_SOCKET)

/** "send" to socket */
static bcmos_errno _bcmos_socket_send(bcmos_msg_queue *queue, uint8_t *buf, uint32_t buf_length)
{
    int rc;
    /* Message queue without remote endpoint address doesn't support transmit */
    if (!queue->ep_extra_data_size)
        return BCM_ERR_NOT_SUPPORTED;
    rc = sendto(queue->ep, buf, buf_length, 0,
        (struct sockaddr *)queue->ep_extra_data, queue->ep_extra_data_size);
    return (rc == buf_length) ? BCM_ERR_OK : BCM_ERR_COMM_FAIL;
}

/** "recv" from socket */
static bcmos_errno _bcmos_socket_recv(bcmos_msg_queue *queue, uint32_t timeout, uint8_t **buf, uint32_t *buf_length)
{
    int rc;
    int wait = 0;

    /* Message queue without local endpoint address doesn't support receive */
    if (!queue->q.parm.local_ep_address)
        return BCM_ERR_NOT_SUPPORTED;

    if (timeout && timeout != BCMOS_WAIT_FOREVER)
    {
        fd_set read_fds;
        struct timeval tv;

        FD_ZERO(&read_fds);
        FD_SET(queue->ep, &read_fds);
        tv.tv_sec = timeout / 1000000;
        tv.tv_usec = (timeout % 1000000) * 1000;
        rc = select(queue->ep + 1, &read_fds, NULL, NULL, &tv);
        if (rc < 0)
        {
            return BCM_ERR_COMM_FAIL;
        }
        if (!rc || !FD_ISSET(queue->ep, &read_fds))
            return BCM_ERR_TIMEOUT;
        wait = MSG_DONTWAIT;
    }

    rc = recv(queue->ep, queue->recv_buf, queue->q.parm.max_mtu, wait);
    if (rc < 0)
    {
        return BCM_ERR_COMM_FAIL;
    }
    if (rc == 0)
        return BCM_ERR_NOENT;

    *buf = queue->recv_buf;
    *buf_length = rc;
    return BCM_ERR_OK;
}


static bcmos_errno _bcmos_socket_close(bcmos_msg_queue *queue)
{
    /* Close domain socket */
    if (queue->ep > 0)
    {
        close(queue->ep);
    }
    if (queue->ep_extra_data)
    {
        bcmos_free(queue->ep_extra_data);
    }
    return BCM_ERR_OK;
}


/* Pack message for over-the-socket transmission.
 * This function is good for case when both queue ends are on the same CPU
 * and there is no need to do any translation.
 */
static bcmos_errno _bcmos_transparent_pack(bcmos_msg_queue *queue, bcmos_msg *msg, uint8_t **buf, uint32_t *buf_length)
{
    uint32_t size = BCMOS_MSG_HDR_SIZE + msg->size;

    if (size > queue->q.parm.max_mtu)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_OVERFLOW, "Attempt to send message longer than configured max_mtu %u > %u\n",
            size, queue->q.parm.max_mtu);
    }
    bcmos_msg_hdr_pack(msg, queue->send_buf, msg->size);
    if (msg->size)
    {
        BUG_ON(msg->data == NULL);
        memcpy(queue->send_buf + BCMOS_MSG_HDR_SIZE, msg->data, msg->size);
    }
    *buf = queue->send_buf;
    *buf_length = size;
    return BCM_ERR_OK;
}

/** "unpack" message. In case of domain socket both queue ends are in the same CPU.
 * Message is sent as-is
 */
static bcmos_errno _bcmos_transparent_unpack(bcmos_msg_queue *queue, uint8_t *buf, uint32_t buf_length, bcmos_msg **msg)
{
    bcmos_msg *m;

    if (buf_length < BCMOS_MSG_HDR_SIZE)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_INTERNAL, "Received message is too short (%u)\n", buf_length);
    }

    /* Adjust buf_length to account for difference in packed and unpacked message header sizes */
    buf_length -= BCMOS_MSG_HDR_SIZE;
    buf_length += sizeof(bcmos_msg);

    m = bcmos_alloc(buf_length);
    if (!m)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_NOMEM, "Received message discarded: %u bytes\n", buf_length);
    }

    bcmos_msg_hdr_unpack(buf, m);
    m->release = NULL;
    m->data_release = NULL;
    if (m->size != buf_length - sizeof(bcmos_msg))
    {
        BCMOS_TRACE_ERR("Received message is insane. Expected data length %u, got %u\n",
            m->size, buf_length - sizeof(bcmos_msg));
        bcmos_free(m);
        return BCM_ERR_INTERNAL;
    }
    if (m->size)
    {
        m->data = m->start = (void *)(m + 1);
        memcpy(m->data, &buf[BCMOS_MSG_HDR_SIZE], m->size);
    }
    else
    {
        m->data = m->start = NULL;
    }
    *msg = m;

    return BCM_ERR_OK;
}


#endif

/* Domain-socket-based inter-process communication */
#ifdef BCMOS_MSG_QUEUE_DOMAIN_SOCKET

bcmos_errno bcmos_sys_msg_queue_domain_socket_open(bcmos_msg_queue *queue)
{
    struct sockaddr_un sa;

    /* Open domain socket */
    queue->ep = socket(AF_UNIX, SOCK_DGRAM, 0);
    if (queue->ep < 0)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't create domain socket. error %s\n", strerror(errno));
    }

    memset(&sa, 0, sizeof(sa));
    sa.sun_family = AF_UNIX;

    /* If local_ep_address is set - the queue supports receive */
    if (queue->q.parm.local_ep_address)
    {
        strncpy(sa.sun_path, queue->q.parm.local_ep_address, sizeof(sa.sun_path) - 1);
        /* In linux path can start from 0 character */
        if (!sa.sun_path[0])
        {
            strncpy(&sa.sun_path[1], &queue->q.parm.local_ep_address[1], sizeof(sa.sun_path) - 1);
        }
        if (bind(queue->ep, (struct sockaddr *)&sa, sizeof(sa)) < 0)
        {
            int err = errno;
            close(queue->ep);
            BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't bind domain socket to %s. error %s\n",
                queue->q.parm.local_ep_address, strerror(err));
        }
    }

    /* If remote_ep_address is set - the queue supports transmit */
    if (queue->q.parm.remote_ep_address)
    {
        queue->ep_extra_data = bcmos_calloc(sizeof(struct sockaddr_un));
        if (!queue->ep_extra_data)
        {
            close(queue->ep);
            return BCM_ERR_NOMEM;
        }
        strncpy(sa.sun_path, queue->q.parm.remote_ep_address, sizeof(sa.sun_path) - 1);
        /* In linux path can start from 0 character */
        if (!sa.sun_path[0])
        {
            strncpy(&sa.sun_path[1], &queue->q.parm.remote_ep_address[1], sizeof(sa.sun_path) - 1);
        }
        memcpy(queue->ep_extra_data, &sa, sizeof(sa));
        queue->ep_extra_data_size = sizeof(sa);
    }

    /* Set callbacks */
    queue->q.parm.close = _bcmos_socket_close;
    queue->q.parm.send = _bcmos_socket_send;
    queue->q.parm.recv = _bcmos_socket_recv;
    if (!queue->q.parm.pack)
        queue->q.parm.pack = _bcmos_transparent_pack;
    if (!queue->q.parm.unpack)
        queue->q.parm.unpack = _bcmos_transparent_unpack;

    return BCM_ERR_OK;
}

#endif

/* UDP-socket-based inter-process communication */
#ifdef BCMOS_MSG_QUEUE_UDP_SOCKET

static bcmos_errno _bcmos_parse_ip_port(const char *s, struct sockaddr_in *sa)
{
    uint32_t ip;
    int n;
    uint32_t ip1, ip2, ip3, ip4, port;

    n = sscanf(s, "%u.%u.%u.%u:%u", &ip1, &ip2, &ip3, &ip4, &port);
    if (n != 5 || ip1 > 0xff || ip2 > 0xff || ip3 > 0xff || ip4 > 0xff || port > 0xffff)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't parse %s. Must be ip_address:port\n", s);
    }
    ip = (ip1 << 24) | (ip2 << 16) | (ip3 << 8) | ip4;
    sa->sin_port = htons(port);
    sa->sin_addr.s_addr = htonl(ip);

    return BCM_ERR_OK;
}

bcmos_errno bcmos_sys_msg_queue_udp_socket_open(bcmos_msg_queue *queue)
{
    struct sockaddr_in sa;
    bcmos_errno rc;

    /* Open UDP socket */
    queue->ep = socket(AF_INET, SOCK_DGRAM, 0);
    if (queue->ep < 0)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't create UDP socket. error %s\n", strerror(errno));
    }

    memset(&sa, 0, sizeof(sa));
    sa.sin_family = AF_INET;

    /* If local_ep_address is set - the queue supports receive */
    if (queue->q.parm.local_ep_address)
    {
        rc = _bcmos_parse_ip_port(queue->q.parm.local_ep_address, &sa);
        if (rc)
            return rc;
        if (!sa.sin_addr.s_addr)
            sa.sin_addr.s_addr = INADDR_ANY;
        if (bind(queue->ep, (struct sockaddr *)&sa, sizeof(sa)) < 0)
        {
            int err = errno;
            close(queue->ep);
            BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't bind UDP socket to %s. error %s\n",
                queue->q.parm.local_ep_address, strerror(err));
        }
    }

    /* If remote_ep_address is set - the queue supports transmit */
    if (queue->q.parm.remote_ep_address)
    {
        rc = _bcmos_parse_ip_port(queue->q.parm.remote_ep_address, &sa);
        if (rc)
            return rc;
        queue->ep_extra_data = bcmos_calloc(sizeof(sa));
        if (!queue->ep_extra_data)
        {
            close(queue->ep);
            return BCM_ERR_NOMEM;
        }
        memcpy(queue->ep_extra_data, &sa, sizeof(sa));
        queue->ep_extra_data_size = sizeof(sa);
    }

    /* Set callbacks */
    queue->q.parm.close = _bcmos_socket_close;
    queue->q.parm.send = _bcmos_socket_send;
    queue->q.parm.recv = _bcmos_socket_recv;
    if (!queue->q.parm.pack)
        queue->q.parm.pack = _bcmos_transparent_pack;
    if (!queue->q.parm.unpack)
        queue->q.parm.unpack = _bcmos_transparent_unpack;

    return BCM_ERR_OK;
}
#endif