Shad Ansari | 2f7f9be | 2017-06-07 13:34:53 -0700 | [diff] [blame^] | 1 | /* |
| 2 | <:copyright-BRCM:2016:DUAL/GPL:standard |
| 3 | |
| 4 | Broadcom Proprietary and Confidential.(c) 2016 Broadcom |
| 5 | All Rights Reserved |
| 6 | |
| 7 | Unless you and Broadcom execute a separate written software license |
| 8 | agreement governing use of this software, this software is licensed |
| 9 | to you under the terms of the GNU General Public License version 2 |
| 10 | (the "GPL"), available at http://www.broadcom.com/licenses/GPLv2.php, |
| 11 | with the following added to such license: |
| 12 | |
| 13 | As a special exception, the copyright holders of this software give |
| 14 | you permission to link this software with independent modules, and |
| 15 | to copy and distribute the resulting executable under terms of your |
| 16 | choice, provided that you also meet, for each linked independent |
| 17 | module, the terms and conditions of the license of that module. |
| 18 | An independent module is a module which is not derived from this |
| 19 | software. The special exception does not apply to any modifications |
| 20 | of the software. |
| 21 | |
| 22 | Not withstanding the above, under no circumstances may you combine |
| 23 | this software in any way with any other Broadcom software provided |
| 24 | under a license other than the GPL, without Broadcom's express prior |
| 25 | written consent. |
| 26 | |
| 27 | :> |
| 28 | */ |
| 29 | |
| 30 | |
| 31 | #include <sys/types.h> |
| 32 | #include <sys/socket.h> |
| 33 | #include <linux/netlink.h> |
| 34 | #include <poll.h> |
| 35 | |
| 36 | #include <bcmos_system.h> |
| 37 | #include <bcmtr_plugin.h> |
| 38 | #include <bcm_config.h> |
| 39 | #include <bcmolt_tr_nl.h> |
| 40 | |
| 41 | #define BCMNL_TXPREFIX_LEN (NLMSG_HDRLEN + sizeof(uint32_t)) |
| 42 | |
| 43 | |
| 44 | /* Buffer list header */ |
| 45 | typedef struct bcmtr_nl_buf bcmtr_nl_buf; |
| 46 | struct bcmtr_nl_buf |
| 47 | { |
| 48 | STAILQ_ENTRY(bcmtr_nl_buf) list; |
| 49 | uint32_t len; /**< Total Length of buffer */ |
| 50 | }; |
| 51 | |
| 52 | /* Plugin channel structure */ |
| 53 | typedef struct bcmtr_nl_channel |
| 54 | { |
| 55 | int device; /* Device */ |
| 56 | STAILQ_HEAD(nl_buf_list, bcmtr_nl_buf) buf_list; |
| 57 | bcmos_sem sem; /* Semaphore that indicates that new buffer is available */ |
| 58 | bcmos_mutex lock; /* lock that protects this structure */ |
| 59 | } bcmtr_nl_channel; |
| 60 | |
| 61 | /* A single NL socket is shared by multiple devices. |
| 62 | * The following control block is used for socket multiplexing |
| 63 | */ |
| 64 | static struct |
| 65 | { |
| 66 | int s; /* Netlink socket */ |
| 67 | int users; /* Number of users */ |
| 68 | bcmos_mutex lock; /* lock that protects this structure */ |
| 69 | bcmos_task nl_rx_task; /* Task that reads from the shared NL socket */ |
| 70 | bcmtr_nl_channel *nlch[BCMTR_MAX_OLTS]; /* Channels */ |
| 71 | } bcmtr_nl_common; |
| 72 | |
| 73 | /* NL socket RX task handler */ |
| 74 | static int bcmtr_nl_rx_task_handler(long data); |
| 75 | |
| 76 | /** Open first communication channel */ |
| 77 | static bcmos_errno bcmtr_nl_open_first(void) |
| 78 | { |
| 79 | struct sockaddr_nl sa = {}; |
| 80 | bcmos_task_parm tp = { |
| 81 | .name = "nl_tx", |
| 82 | .handler = bcmtr_nl_rx_task_handler, |
| 83 | .priority = TASK_PRIORITY_TRANSPORT_RX |
| 84 | }; |
| 85 | int s; |
| 86 | bcmos_errno err; |
| 87 | |
| 88 | s = socket(AF_NETLINK, SOCK_RAW, BCMTR_NL_FAMILY); |
| 89 | if (s < 0) |
| 90 | { |
| 91 | BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't create NL socket\n"); |
| 92 | } |
| 93 | |
| 94 | /* Connect to remote */ |
| 95 | sa.nl_pid = 0; |
| 96 | if (connect(s, (struct sockaddr*)&sa, sizeof(sa)) == -1) |
| 97 | { |
| 98 | close(s); |
| 99 | BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't connect NL socket. error %d (%s)\n", |
| 100 | errno, strerror(errno)); |
| 101 | } |
| 102 | |
| 103 | /* Bind local */ |
| 104 | sa.nl_family = AF_NETLINK; |
| 105 | sa.nl_pid = getpid(); |
| 106 | if (bind(s, (struct sockaddr*)&sa, sizeof(sa) ) == -1) |
| 107 | { |
| 108 | close(s); |
| 109 | BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't bind NL socket. error %d (%s)\n", |
| 110 | errno, strerror(errno)); |
| 111 | } |
| 112 | |
| 113 | bcmtr_nl_common.s = s; |
| 114 | |
| 115 | /* Create RX thread */ |
| 116 | err = bcmos_task_create(&bcmtr_nl_common.nl_rx_task, &tp); |
| 117 | if (err) |
| 118 | { |
| 119 | close(s); |
| 120 | bcmtr_nl_common.s = 0; |
| 121 | BCMOS_TRACE_RETURN(BCM_ERR_COMM_FAIL, "Can't create NL RX thread. error %d (%s)\n", |
| 122 | err, bcmos_strerror(err)); |
| 123 | } |
| 124 | |
| 125 | return BCM_ERR_OK; |
| 126 | } |
| 127 | |
| 128 | /** Close last communication channel */ |
| 129 | static bcmos_errno bcmtr_nl_close_last(void) |
| 130 | { |
| 131 | close(bcmtr_nl_common.s); |
| 132 | return bcmos_task_destroy(&bcmtr_nl_common.nl_rx_task); |
| 133 | } |
| 134 | |
| 135 | /** Open communication channel */ |
| 136 | static bcmos_errno bcmtr_nl_open(int device, bcmtr_plugin_cfg *cfg, bcmtr_plugin_channel *ch) |
| 137 | { |
| 138 | bcmtr_nl_channel *nlch; |
| 139 | bcmos_errno err = BCM_ERR_OK; |
| 140 | |
| 141 | if ((unsigned)device >= BCMTR_MAX_OLTS) |
| 142 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Device %d is invalid\n"); |
| 143 | |
| 144 | if (bcmtr_nl_common.nlch[device]) |
| 145 | BCMOS_TRACE_RETURN(BCM_ERR_ALREADY, "Channel for device %d is already opened\n"); |
| 146 | |
| 147 | nlch = bcmos_calloc(sizeof(*nlch)); |
| 148 | if (!nlch) |
| 149 | { |
| 150 | BCMOS_TRACE_RETURN(BCM_ERR_NOMEM, "No memory\n"); |
| 151 | } |
| 152 | nlch->device = device; |
| 153 | STAILQ_INIT(&nlch->buf_list); |
| 154 | err = bcmos_mutex_create(&nlch->lock, 0, "tr_nlch"); |
| 155 | err = err ? err : bcmos_sem_create(&nlch->sem, 0, 0, "tr_nlch"); |
| 156 | |
| 157 | /* Tie in with the control block shared by all devices */ |
| 158 | bcmos_mutex_lock(&bcmtr_nl_common.lock); |
| 159 | if (!bcmtr_nl_common.users) |
| 160 | err = err ? err : bcmtr_nl_open_first(); |
| 161 | if (err) |
| 162 | { |
| 163 | bcmos_mutex_unlock(&bcmtr_nl_common.lock); |
| 164 | bcmos_free(nlch); |
| 165 | BCMOS_TRACE_RETURN(err, "Failed to open or bind NL socket\n"); |
| 166 | } |
| 167 | ++bcmtr_nl_common.users; |
| 168 | bcmtr_nl_common.nlch[device] = nlch; |
| 169 | bcmos_mutex_unlock(&bcmtr_nl_common.lock); |
| 170 | |
| 171 | *ch = (bcmtr_plugin_channel)nlch; |
| 172 | |
| 173 | return BCM_ERR_OK; |
| 174 | } |
| 175 | |
| 176 | /** Close communication channel */ |
| 177 | static bcmos_errno bcmtr_nl_close(bcmtr_plugin_channel ch) |
| 178 | { |
| 179 | bcmtr_nl_channel *nlch = (bcmtr_nl_channel *)ch; |
| 180 | bcmtr_nl_buf *nl_buf, *nl_buf_tmp; |
| 181 | bcmolt_buf buf; |
| 182 | bcmos_errno err = BCM_ERR_OK; |
| 183 | |
| 184 | bcmos_mutex_lock(&bcmtr_nl_common.lock); |
| 185 | if (bcmtr_nl_common.nlch[nlch->device] == nlch) |
| 186 | { |
| 187 | bcmtr_nl_common.nlch[nlch->device] = NULL; |
| 188 | --bcmtr_nl_common.users; |
| 189 | if (bcmtr_nl_common.users) |
| 190 | err = bcmtr_nl_close_last(); |
| 191 | } |
| 192 | else |
| 193 | { |
| 194 | BCMOS_TRACE_ERR("Unexpected NL channel\n"); |
| 195 | err = BCM_ERR_INTERNAL; |
| 196 | } |
| 197 | bcmos_mutex_unlock(&bcmtr_nl_common.lock); |
| 198 | bcmos_mutex_destroy(&nlch->lock); |
| 199 | bcmos_sem_destroy(&nlch->sem); |
| 200 | STAILQ_FOREACH_SAFE(nl_buf, &nlch->buf_list, list, nl_buf_tmp) |
| 201 | { |
| 202 | STAILQ_REMOVE_HEAD(&nlch->buf_list, list); |
| 203 | bcmolt_buf_init(&buf, nl_buf->len, (uint8_t *)nl_buf, BCMTR_BUF_ENDIAN); |
| 204 | bcmolt_buf_free(&buf); |
| 205 | } |
| 206 | bcmos_free(nlch); |
| 207 | |
| 208 | return err; |
| 209 | } |
| 210 | |
| 211 | /** Send data */ |
| 212 | static bcmos_errno bcmtr_nl_send(bcmtr_plugin_channel ch, bcmolt_subchannel subch, bcmolt_buf *buf, |
| 213 | bcmtr_send_flags flags) |
| 214 | { |
| 215 | bcmtr_nl_channel *nlch = (bcmtr_nl_channel *)ch; |
| 216 | int total_len = bcmolt_buf_get_used(buf); |
| 217 | struct nlmsghdr *nlmsg_hdr = (struct nlmsghdr *)(long)buf->start; |
| 218 | struct iovec iov = { buf->start, .iov_len=total_len }; |
| 219 | struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 }; |
| 220 | int rc; |
| 221 | |
| 222 | nlmsg_hdr->nlmsg_len = total_len; |
| 223 | nlmsg_hdr->nlmsg_pid = getpid(); |
| 224 | buf->start[NLMSG_HDRLEN] = nlch->device; |
| 225 | |
| 226 | rc = sendmsg(bcmtr_nl_common.s, &msg, 0); |
| 227 | |
| 228 | return (rc == total_len) ? BCM_ERR_OK : BCM_ERR_COMM_FAIL; |
| 229 | } |
| 230 | |
| 231 | /* NL socket RX task handler */ |
| 232 | static int bcmtr_nl_rx_task_handler(long data) |
| 233 | { |
| 234 | bcmos_task *my_task = bcmos_task_current(); |
| 235 | struct iovec iov; |
| 236 | struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1 }; |
| 237 | uint32_t device; |
| 238 | bcmolt_buf buf = {}; |
| 239 | bcmtr_nl_buf *nl_buf; |
| 240 | bcmtr_nl_channel *nlch; |
| 241 | bcmos_errno err; |
| 242 | int rc; |
| 243 | |
| 244 | while (!my_task->destroy_request) |
| 245 | { |
| 246 | err = bcmolt_buf_alloc(&buf, BCMTR_MAX_MTU_SIZE + BCMNL_TXPREFIX_LEN, BCMTR_BUF_ENDIAN); |
| 247 | if (err) |
| 248 | { |
| 249 | /* We are out of memory, hopefully temporary. Wait and continue */ |
| 250 | bcmos_usleep(1000); |
| 251 | continue; |
| 252 | } |
| 253 | iov.iov_base = buf.start; |
| 254 | iov.iov_len = buf.len; |
| 255 | |
| 256 | /* Wait for message */ |
| 257 | rc = recvmsg(bcmtr_nl_common.s, &msg, 0); |
| 258 | if (rc < NLMSG_HDRLEN) |
| 259 | { |
| 260 | BCMOS_TRACE_ERR("Fatal error. recvmsg() failed with error %d (%s)\n", errno, strerror(errno)); |
| 261 | break; |
| 262 | } |
| 263 | |
| 264 | /* Device follows NL header */ |
| 265 | memcpy(&device, buf.start + NLMSG_HDRLEN, sizeof(uint32_t)); |
| 266 | if (device >= BCMTR_MAX_OLTS || !(nlch = bcmtr_nl_common.nlch[device])) |
| 267 | { |
| 268 | BCMOS_TRACE_ERR("Got message from unexpected device %u. Ignored\n", device); |
| 269 | bcmolt_buf_free(&buf); |
| 270 | continue; |
| 271 | } |
| 272 | |
| 273 | /* Put message on the channel's queue */ |
| 274 | nl_buf = (bcmtr_nl_buf *)(long)buf.start; |
| 275 | nl_buf->len = rc; |
| 276 | bcmos_mutex_lock(&nlch->lock); |
| 277 | STAILQ_INSERT_TAIL(&nlch->buf_list, nl_buf, list); |
| 278 | bcmos_mutex_unlock(&nlch->lock); |
| 279 | bcmos_sem_post(&nlch->sem); |
| 280 | |
| 281 | /* Buffer is in use elsewhere */ |
| 282 | buf.start = buf.curr = NULL; |
| 283 | } |
| 284 | |
| 285 | bcmolt_buf_free(&buf); |
| 286 | my_task->destroyed = BCMOS_TRUE; |
| 287 | return 0; |
| 288 | } |
| 289 | |
| 290 | /** Receive data */ |
| 291 | static bcmos_errno bcmtr_nl_recv(bcmtr_plugin_channel ch, bcmolt_subchannel *subch, bcmolt_buf *buf) |
| 292 | { |
| 293 | bcmtr_nl_channel *nlch = (bcmtr_nl_channel *)ch; |
| 294 | bcmtr_nl_buf *nl_buf; |
| 295 | bcmos_errno err; |
| 296 | |
| 297 | err = bcmos_sem_wait(&nlch->sem, BCMTR_MSG_TIMEOUT * 1000); |
| 298 | if (err) |
| 299 | return err; |
| 300 | bcmos_mutex_lock(&nlch->lock); |
| 301 | nl_buf = STAILQ_FIRST(&nlch->buf_list); |
| 302 | if (!nl_buf) |
| 303 | { |
| 304 | bcmos_mutex_unlock(&nlch->lock); |
| 305 | return BCM_ERR_COMM_FAIL; |
| 306 | } |
| 307 | STAILQ_REMOVE_HEAD(&nlch->buf_list, list); |
| 308 | bcmos_mutex_unlock(&nlch->lock); |
| 309 | |
| 310 | /* Got buffer */ |
| 311 | bcmolt_buf_init(buf, nl_buf->len, (uint8_t *)nl_buf, BCMTR_BUF_ENDIAN); |
| 312 | buf->curr = buf->start + BCMNL_TXPREFIX_LEN; |
| 313 | |
| 314 | *subch = 0; |
| 315 | |
| 316 | return BCM_ERR_OK; |
| 317 | } |
| 318 | |
| 319 | /** Initialize plugin callbacks |
| 320 | * \param[in,out] driver Transport plugin driver structure |
| 321 | * \return error code |
| 322 | */ |
| 323 | bcmos_errno bcmtr_nl_plugin_init(bcmtr_plugin_cfg *cfg, bcmtr_driver *driver) |
| 324 | { |
| 325 | bcmos_errno err; |
| 326 | |
| 327 | bcmos_printf("Launching with a netlink connection\n"); |
| 328 | |
| 329 | driver->open = bcmtr_nl_open; |
| 330 | driver->close = bcmtr_nl_close; |
| 331 | driver->recv = bcmtr_nl_recv; |
| 332 | driver->send = bcmtr_nl_send; |
| 333 | cfg->headroom = BCMNL_TXPREFIX_LEN; |
| 334 | |
| 335 | err = bcmos_mutex_create(&bcmtr_nl_common.lock, 0, "tr_nl"); |
| 336 | |
| 337 | return err; |
| 338 | } |
| 339 | |