| /* |
| <: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 <sys/types.h> |
| #include <sys/socket.h> |
| #include <linux/netlink.h> |
| #include <poll.h> |
| |
| #include <bcmos_system.h> |
| #include <bcmtr_plugin.h> |
| #include <bcm_config.h> |
| #include <bcmolt_tr_nl.h> |
| |
| #define BCMNL_TXPREFIX_LEN (NLMSG_HDRLEN + sizeof(uint32_t)) |
| |
| |
| /* Buffer list header */ |
| typedef struct bcmtr_nl_buf bcmtr_nl_buf; |
| struct bcmtr_nl_buf |
| { |
| STAILQ_ENTRY(bcmtr_nl_buf) list; |
| uint32_t len; /**< Total Length of buffer */ |
| }; |
| |
| /* Plugin channel structure */ |
| typedef struct bcmtr_nl_channel |
| { |
| int device; /* Device */ |
| STAILQ_HEAD(nl_buf_list, bcmtr_nl_buf) buf_list; |
| bcmos_sem sem; /* Semaphore that indicates that new buffer is available */ |
| bcmos_mutex lock; /* lock that protects this structure */ |
| } bcmtr_nl_channel; |
| |
| /* A single NL socket is shared by multiple devices. |
| * The following control block is used for socket multiplexing |
| */ |
| static struct |
| { |
| int s; /* Netlink socket */ |
| int users; /* Number of users */ |
| bcmos_mutex lock; /* lock that protects this structure */ |
| bcmos_task nl_rx_task; /* Task that reads from the shared NL socket */ |
| bcmtr_nl_channel *nlch[BCMTR_MAX_OLTS]; /* Channels */ |
| } bcmtr_nl_common; |
| |
| /* NL socket RX task handler */ |
| static int bcmtr_nl_rx_task_handler(long data); |
| |
| /** Open first communication channel */ |
| static bcmos_errno bcmtr_nl_open_first(void) |
| { |
| struct sockaddr_nl sa = {}; |
| bcmos_task_parm tp = { |
| .name = "nl_tx", |
| .handler = bcmtr_nl_rx_task_handler, |
| .priority = TASK_PRIORITY_TRANSPORT_RX |
| }; |
| int s; |
| bcmos_errno err; |
| |
| s = socket(AF_NETLINK, SOCK_RAW, BCMTR_NL_FAMILY); |
| if (s < 0) |
| { |
| BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't create NL socket\n"); |
| } |
| |
| /* Connect to remote */ |
| sa.nl_pid = 0; |
| if (connect(s, (struct sockaddr*)&sa, sizeof(sa)) == -1) |
| { |
| close(s); |
| BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't connect NL socket. error %d (%s)\n", |
| errno, strerror(errno)); |
| } |
| |
| /* Bind local */ |
| sa.nl_family = AF_NETLINK; |
| sa.nl_pid = getpid(); |
| if (bind(s, (struct sockaddr*)&sa, sizeof(sa) ) == -1) |
| { |
| close(s); |
| BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't bind NL socket. error %d (%s)\n", |
| errno, strerror(errno)); |
| } |
| |
| bcmtr_nl_common.s = s; |
| |
| /* Create RX thread */ |
| err = bcmos_task_create(&bcmtr_nl_common.nl_rx_task, &tp); |
| if (err) |
| { |
| close(s); |
| bcmtr_nl_common.s = 0; |
| BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't create NL RX thread. error %d (%s)\n", |
| err, bcmos_strerror(err)); |
| } |
| |
| return BCM_ERR_OK; |
| } |
| |
| /** Close last communication channel */ |
| static bcmos_errno bcmtr_nl_close_last(void) |
| { |
| close(bcmtr_nl_common.s); |
| return bcmos_task_destroy(&bcmtr_nl_common.nl_rx_task); |
| } |
| |
| /** Open communication channel */ |
| static bcmos_errno bcmtr_nl_open(int device, bcmtr_plugin_cfg *cfg, bcmtr_plugin_channel *ch) |
| { |
| bcmtr_nl_channel *nlch; |
| bcmos_errno err = BCM_ERR_OK; |
| |
| if ((unsigned)device >= BCMTR_MAX_OLTS) |
| BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Device %d is invalid\n"); |
| |
| if (bcmtr_nl_common.nlch[device]) |
| BCMOS_TRACE_RETURN(BCM_ERR_ALREADY, "Channel for device %d is already opened\n"); |
| |
| nlch = bcmos_calloc(sizeof(*nlch)); |
| if (!nlch) |
| { |
| BCMOS_TRACE_RETURN(BCM_ERR_NOMEM, "No memory\n"); |
| } |
| nlch->device = device; |
| STAILQ_INIT(&nlch->buf_list); |
| err = bcmos_mutex_create(&nlch->lock, 0, "tr_nlch"); |
| err = err ? err : bcmos_sem_create(&nlch->sem, 0, 0, "tr_nlch"); |
| |
| /* Tie in with the control block shared by all devices */ |
| bcmos_mutex_lock(&bcmtr_nl_common.lock); |
| if (!bcmtr_nl_common.users) |
| err = err ? err : bcmtr_nl_open_first(); |
| if (err) |
| { |
| bcmos_mutex_unlock(&bcmtr_nl_common.lock); |
| bcmos_free(nlch); |
| BCMOS_TRACE_RETURN(err, "Failed to open or bind NL socket\n"); |
| } |
| ++bcmtr_nl_common.users; |
| bcmtr_nl_common.nlch[device] = nlch; |
| bcmos_mutex_unlock(&bcmtr_nl_common.lock); |
| |
| *ch = (bcmtr_plugin_channel)nlch; |
| |
| return BCM_ERR_OK; |
| } |
| |
| /** Close communication channel */ |
| static bcmos_errno bcmtr_nl_close(bcmtr_plugin_channel ch) |
| { |
| bcmtr_nl_channel *nlch = (bcmtr_nl_channel *)ch; |
| bcmtr_nl_buf *nl_buf, *nl_buf_tmp; |
| bcmolt_buf buf; |
| bcmos_errno err = BCM_ERR_OK; |
| |
| bcmos_mutex_lock(&bcmtr_nl_common.lock); |
| if (bcmtr_nl_common.nlch[nlch->device] == nlch) |
| { |
| bcmtr_nl_common.nlch[nlch->device] = NULL; |
| --bcmtr_nl_common.users; |
| if (bcmtr_nl_common.users) |
| err = bcmtr_nl_close_last(); |
| } |
| else |
| { |
| BCMOS_TRACE_ERR("Unexpected NL channel\n"); |
| err = BCM_ERR_INTERNAL; |
| } |
| bcmos_mutex_unlock(&bcmtr_nl_common.lock); |
| bcmos_mutex_destroy(&nlch->lock); |
| bcmos_sem_destroy(&nlch->sem); |
| STAILQ_FOREACH_SAFE(nl_buf, &nlch->buf_list, list, nl_buf_tmp) |
| { |
| STAILQ_REMOVE_HEAD(&nlch->buf_list, list); |
| bcmolt_buf_init(&buf, nl_buf->len, (uint8_t *)nl_buf, BCMTR_BUF_ENDIAN); |
| bcmolt_buf_free(&buf); |
| } |
| bcmos_free(nlch); |
| |
| return err; |
| } |
| |
| /** Send data */ |
| static bcmos_errno bcmtr_nl_send(bcmtr_plugin_channel ch, bcmolt_subchannel subch, bcmolt_buf *buf, |
| bcmtr_send_flags flags) |
| { |
| bcmtr_nl_channel *nlch = (bcmtr_nl_channel *)ch; |
| int total_len = bcmolt_buf_get_used(buf); |
| struct nlmsghdr *nlmsg_hdr = (struct nlmsghdr *)(long)buf->start; |
| struct iovec iov = { buf->start, .iov_len=total_len }; |
| struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 }; |
| int rc; |
| |
| nlmsg_hdr->nlmsg_len = total_len; |
| nlmsg_hdr->nlmsg_pid = getpid(); |
| buf->start[NLMSG_HDRLEN] = nlch->device; |
| |
| rc = sendmsg(bcmtr_nl_common.s, &msg, 0); |
| |
| return (rc == total_len) ? BCM_ERR_OK : BCM_ERR_COMM_FAIL; |
| } |
| |
| /* NL socket RX task handler */ |
| static int bcmtr_nl_rx_task_handler(long data) |
| { |
| bcmos_task *my_task = bcmos_task_current(); |
| struct iovec iov; |
| struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 }; |
| uint32_t device; |
| bcmolt_buf buf = {}; |
| bcmtr_nl_buf *nl_buf; |
| bcmtr_nl_channel *nlch; |
| bcmos_errno err; |
| int rc; |
| |
| while (!my_task->destroy_request) |
| { |
| err = bcmolt_buf_alloc(&buf, BCMTR_MAX_MTU_SIZE + BCMNL_TXPREFIX_LEN, BCMTR_BUF_ENDIAN); |
| if (err) |
| { |
| /* We are out of memory, hopefully temporary. Wait and continue */ |
| bcmos_usleep(1000); |
| continue; |
| } |
| iov.iov_base = buf.start; |
| iov.iov_len = buf.len; |
| |
| /* Wait for message */ |
| rc = recvmsg(bcmtr_nl_common.s, &msg, 0); |
| if (rc < NLMSG_HDRLEN) |
| { |
| BCMOS_TRACE_ERR("Fatal error. recvmsg() failed with error %d (%s)\n", errno, strerror(errno)); |
| break; |
| } |
| |
| /* Device follows NL header */ |
| memcpy(&device, buf.start + NLMSG_HDRLEN, sizeof(uint32_t)); |
| if (device >= BCMTR_MAX_OLTS || !(nlch = bcmtr_nl_common.nlch[device])) |
| { |
| BCMOS_TRACE_ERR("Got message from unexpected device %u. Ignored\n", device); |
| bcmolt_buf_free(&buf); |
| continue; |
| } |
| |
| /* Put message on the channel's queue */ |
| nl_buf = (bcmtr_nl_buf *)(long)buf.start; |
| nl_buf->len = rc; |
| bcmos_mutex_lock(&nlch->lock); |
| STAILQ_INSERT_TAIL(&nlch->buf_list, nl_buf, list); |
| bcmos_mutex_unlock(&nlch->lock); |
| bcmos_sem_post(&nlch->sem); |
| |
| /* Buffer is in use elsewhere */ |
| buf.start = buf.curr = NULL; |
| } |
| |
| bcmolt_buf_free(&buf); |
| my_task->destroyed = BCMOS_TRUE; |
| return 0; |
| } |
| |
| /** Receive data */ |
| static bcmos_errno bcmtr_nl_recv(bcmtr_plugin_channel ch, bcmolt_subchannel *subch, bcmolt_buf *buf) |
| { |
| bcmtr_nl_channel *nlch = (bcmtr_nl_channel *)ch; |
| bcmtr_nl_buf *nl_buf; |
| bcmos_errno err; |
| |
| err = bcmos_sem_wait(&nlch->sem, BCMTR_MSG_TIMEOUT * 1000); |
| if (err) |
| return err; |
| bcmos_mutex_lock(&nlch->lock); |
| nl_buf = STAILQ_FIRST(&nlch->buf_list); |
| if (!nl_buf) |
| { |
| bcmos_mutex_unlock(&nlch->lock); |
| return BCM_ERR_COMM_FAIL; |
| } |
| STAILQ_REMOVE_HEAD(&nlch->buf_list, list); |
| bcmos_mutex_unlock(&nlch->lock); |
| |
| /* Got buffer */ |
| bcmolt_buf_init(buf, nl_buf->len, (uint8_t *)nl_buf, BCMTR_BUF_ENDIAN); |
| buf->curr = buf->start + BCMNL_TXPREFIX_LEN; |
| |
| *subch = 0; |
| |
| return BCM_ERR_OK; |
| } |
| |
| /** Initialize plugin callbacks |
| * \param[in,out] driver Transport plugin driver structure |
| * \return error code |
| */ |
| bcmos_errno bcmtr_nl_plugin_init(bcmtr_plugin_cfg *cfg, bcmtr_driver *driver) |
| { |
| bcmos_errno err; |
| |
| bcmos_printf("Launching with a netlink connection\n"); |
| |
| driver->open = bcmtr_nl_open; |
| driver->close = bcmtr_nl_close; |
| driver->recv = bcmtr_nl_recv; |
| driver->send = bcmtr_nl_send; |
| cfg->headroom = BCMNL_TXPREFIX_LEN; |
| |
| err = bcmos_mutex_create(&bcmtr_nl_common.lock, 0, "tr_nl"); |
| |
| return err; |
| } |
| |