Gavin Shan | 2d283bd | 2016-07-19 11:54:16 +1000 | [diff] [blame^] | 1 | /* |
| 2 | * Copyright Gavin Shan, IBM Corporation 2016. |
| 3 | * |
| 4 | * This program is free software; you can redistribute it and/or modify |
| 5 | * it under the terms of the GNU General Public License as published by |
| 6 | * the Free Software Foundation; either version 2 of the License, or |
| 7 | * (at your option) any later version. |
| 8 | */ |
| 9 | |
| 10 | #include <linux/module.h> |
| 11 | #include <linux/kernel.h> |
| 12 | #include <linux/init.h> |
| 13 | #include <linux/netdevice.h> |
| 14 | #include <linux/skbuff.h> |
| 15 | #include <linux/netlink.h> |
| 16 | |
| 17 | #include <net/ncsi.h> |
| 18 | #include <net/net_namespace.h> |
| 19 | #include <net/sock.h> |
| 20 | |
| 21 | #include "internal.h" |
| 22 | |
| 23 | LIST_HEAD(ncsi_dev_list); |
| 24 | DEFINE_SPINLOCK(ncsi_dev_lock); |
| 25 | |
| 26 | static inline int ncsi_filter_size(int table) |
| 27 | { |
| 28 | int sizes[] = { 2, 6, 6, 6 }; |
| 29 | |
| 30 | BUILD_BUG_ON(ARRAY_SIZE(sizes) != NCSI_FILTER_MAX); |
| 31 | if (table < NCSI_FILTER_BASE || table >= NCSI_FILTER_MAX) |
| 32 | return -EINVAL; |
| 33 | |
| 34 | return sizes[table]; |
| 35 | } |
| 36 | |
| 37 | int ncsi_find_filter(struct ncsi_channel *nc, int table, void *data) |
| 38 | { |
| 39 | struct ncsi_channel_filter *ncf; |
| 40 | void *bitmap; |
| 41 | int index, size; |
| 42 | unsigned long flags; |
| 43 | |
| 44 | ncf = nc->filters[table]; |
| 45 | if (!ncf) |
| 46 | return -ENXIO; |
| 47 | |
| 48 | size = ncsi_filter_size(table); |
| 49 | if (size < 0) |
| 50 | return size; |
| 51 | |
| 52 | spin_lock_irqsave(&nc->lock, flags); |
| 53 | bitmap = (void *)&ncf->bitmap; |
| 54 | index = -1; |
| 55 | while ((index = find_next_bit(bitmap, ncf->total, index + 1)) |
| 56 | < ncf->total) { |
| 57 | if (!memcmp(ncf->data + size * index, data, size)) { |
| 58 | spin_unlock_irqrestore(&nc->lock, flags); |
| 59 | return index; |
| 60 | } |
| 61 | } |
| 62 | spin_unlock_irqrestore(&nc->lock, flags); |
| 63 | |
| 64 | return -ENOENT; |
| 65 | } |
| 66 | |
| 67 | int ncsi_add_filter(struct ncsi_channel *nc, int table, void *data) |
| 68 | { |
| 69 | struct ncsi_channel_filter *ncf; |
| 70 | int index, size; |
| 71 | void *bitmap; |
| 72 | unsigned long flags; |
| 73 | |
| 74 | size = ncsi_filter_size(table); |
| 75 | if (size < 0) |
| 76 | return size; |
| 77 | |
| 78 | index = ncsi_find_filter(nc, table, data); |
| 79 | if (index >= 0) |
| 80 | return index; |
| 81 | |
| 82 | ncf = nc->filters[table]; |
| 83 | if (!ncf) |
| 84 | return -ENODEV; |
| 85 | |
| 86 | spin_lock_irqsave(&nc->lock, flags); |
| 87 | bitmap = (void *)&ncf->bitmap; |
| 88 | do { |
| 89 | index = find_next_zero_bit(bitmap, ncf->total, 0); |
| 90 | if (index >= ncf->total) { |
| 91 | spin_unlock_irqrestore(&nc->lock, flags); |
| 92 | return -ENOSPC; |
| 93 | } |
| 94 | } while (test_and_set_bit(index, bitmap)); |
| 95 | |
| 96 | memcpy(ncf->data + size * index, data, size); |
| 97 | spin_unlock_irqrestore(&nc->lock, flags); |
| 98 | |
| 99 | return index; |
| 100 | } |
| 101 | |
| 102 | int ncsi_remove_filter(struct ncsi_channel *nc, int table, int index) |
| 103 | { |
| 104 | struct ncsi_channel_filter *ncf; |
| 105 | int size; |
| 106 | void *bitmap; |
| 107 | unsigned long flags; |
| 108 | |
| 109 | size = ncsi_filter_size(table); |
| 110 | if (size < 0) |
| 111 | return size; |
| 112 | |
| 113 | ncf = nc->filters[table]; |
| 114 | if (!ncf || index >= ncf->total) |
| 115 | return -ENODEV; |
| 116 | |
| 117 | spin_lock_irqsave(&nc->lock, flags); |
| 118 | bitmap = (void *)&ncf->bitmap; |
| 119 | if (test_and_clear_bit(index, bitmap)) |
| 120 | memset(ncf->data + size * index, 0, size); |
| 121 | spin_unlock_irqrestore(&nc->lock, flags); |
| 122 | |
| 123 | return 0; |
| 124 | } |
| 125 | |
| 126 | struct ncsi_channel *ncsi_find_channel(struct ncsi_package *np, |
| 127 | unsigned char id) |
| 128 | { |
| 129 | struct ncsi_channel *nc; |
| 130 | |
| 131 | NCSI_FOR_EACH_CHANNEL(np, nc) { |
| 132 | if (nc->id == id) |
| 133 | return nc; |
| 134 | } |
| 135 | |
| 136 | return NULL; |
| 137 | } |
| 138 | |
| 139 | struct ncsi_channel *ncsi_add_channel(struct ncsi_package *np, unsigned char id) |
| 140 | { |
| 141 | struct ncsi_channel *nc, *tmp; |
| 142 | int index; |
| 143 | unsigned long flags; |
| 144 | |
| 145 | nc = kzalloc(sizeof(*nc), GFP_ATOMIC); |
| 146 | if (!nc) |
| 147 | return NULL; |
| 148 | |
| 149 | nc->id = id; |
| 150 | nc->package = np; |
| 151 | nc->state = NCSI_CHANNEL_INACTIVE; |
| 152 | spin_lock_init(&nc->lock); |
| 153 | for (index = 0; index < NCSI_CAP_MAX; index++) |
| 154 | nc->caps[index].index = index; |
| 155 | for (index = 0; index < NCSI_MODE_MAX; index++) |
| 156 | nc->modes[index].index = index; |
| 157 | |
| 158 | spin_lock_irqsave(&np->lock, flags); |
| 159 | tmp = ncsi_find_channel(np, id); |
| 160 | if (tmp) { |
| 161 | spin_unlock_irqrestore(&np->lock, flags); |
| 162 | kfree(nc); |
| 163 | return tmp; |
| 164 | } |
| 165 | |
| 166 | list_add_tail_rcu(&nc->node, &np->channels); |
| 167 | np->channel_num++; |
| 168 | spin_unlock_irqrestore(&np->lock, flags); |
| 169 | |
| 170 | return nc; |
| 171 | } |
| 172 | |
| 173 | static void ncsi_remove_channel(struct ncsi_channel *nc) |
| 174 | { |
| 175 | struct ncsi_package *np = nc->package; |
| 176 | struct ncsi_channel_filter *ncf; |
| 177 | unsigned long flags; |
| 178 | int i; |
| 179 | |
| 180 | /* Release filters */ |
| 181 | spin_lock_irqsave(&nc->lock, flags); |
| 182 | for (i = 0; i < NCSI_FILTER_MAX; i++) { |
| 183 | ncf = nc->filters[i]; |
| 184 | if (!ncf) |
| 185 | continue; |
| 186 | |
| 187 | nc->filters[i] = NULL; |
| 188 | kfree(ncf); |
| 189 | } |
| 190 | |
| 191 | nc->state = NCSI_CHANNEL_INACTIVE; |
| 192 | spin_unlock_irqrestore(&nc->lock, flags); |
| 193 | |
| 194 | /* Remove and free channel */ |
| 195 | spin_lock_irqsave(&np->lock, flags); |
| 196 | list_del_rcu(&nc->node); |
| 197 | np->channel_num--; |
| 198 | spin_unlock_irqrestore(&np->lock, flags); |
| 199 | |
| 200 | kfree(nc); |
| 201 | } |
| 202 | |
| 203 | struct ncsi_package *ncsi_find_package(struct ncsi_dev_priv *ndp, |
| 204 | unsigned char id) |
| 205 | { |
| 206 | struct ncsi_package *np; |
| 207 | |
| 208 | NCSI_FOR_EACH_PACKAGE(ndp, np) { |
| 209 | if (np->id == id) |
| 210 | return np; |
| 211 | } |
| 212 | |
| 213 | return NULL; |
| 214 | } |
| 215 | |
| 216 | struct ncsi_package *ncsi_add_package(struct ncsi_dev_priv *ndp, |
| 217 | unsigned char id) |
| 218 | { |
| 219 | struct ncsi_package *np, *tmp; |
| 220 | unsigned long flags; |
| 221 | |
| 222 | np = kzalloc(sizeof(*np), GFP_ATOMIC); |
| 223 | if (!np) |
| 224 | return NULL; |
| 225 | |
| 226 | np->id = id; |
| 227 | np->ndp = ndp; |
| 228 | spin_lock_init(&np->lock); |
| 229 | INIT_LIST_HEAD(&np->channels); |
| 230 | |
| 231 | spin_lock_irqsave(&ndp->lock, flags); |
| 232 | tmp = ncsi_find_package(ndp, id); |
| 233 | if (tmp) { |
| 234 | spin_unlock_irqrestore(&ndp->lock, flags); |
| 235 | kfree(np); |
| 236 | return tmp; |
| 237 | } |
| 238 | |
| 239 | list_add_tail_rcu(&np->node, &ndp->packages); |
| 240 | ndp->package_num++; |
| 241 | spin_unlock_irqrestore(&ndp->lock, flags); |
| 242 | |
| 243 | return np; |
| 244 | } |
| 245 | |
| 246 | void ncsi_remove_package(struct ncsi_package *np) |
| 247 | { |
| 248 | struct ncsi_dev_priv *ndp = np->ndp; |
| 249 | struct ncsi_channel *nc, *tmp; |
| 250 | unsigned long flags; |
| 251 | |
| 252 | /* Release all child channels */ |
| 253 | list_for_each_entry_safe(nc, tmp, &np->channels, node) |
| 254 | ncsi_remove_channel(nc); |
| 255 | |
| 256 | /* Remove and free package */ |
| 257 | spin_lock_irqsave(&ndp->lock, flags); |
| 258 | list_del_rcu(&np->node); |
| 259 | ndp->package_num--; |
| 260 | spin_unlock_irqrestore(&ndp->lock, flags); |
| 261 | |
| 262 | kfree(np); |
| 263 | } |
| 264 | |
| 265 | void ncsi_find_package_and_channel(struct ncsi_dev_priv *ndp, |
| 266 | unsigned char id, |
| 267 | struct ncsi_package **np, |
| 268 | struct ncsi_channel **nc) |
| 269 | { |
| 270 | struct ncsi_package *p; |
| 271 | struct ncsi_channel *c; |
| 272 | |
| 273 | p = ncsi_find_package(ndp, NCSI_PACKAGE_INDEX(id)); |
| 274 | c = p ? ncsi_find_channel(p, NCSI_CHANNEL_INDEX(id)) : NULL; |
| 275 | |
| 276 | if (np) |
| 277 | *np = p; |
| 278 | if (nc) |
| 279 | *nc = c; |
| 280 | } |
| 281 | |
| 282 | /* For two consecutive NCSI commands, the packet IDs shouldn't |
| 283 | * be same. Otherwise, the bogus response might be replied. So |
| 284 | * the available IDs are allocated in round-robin fashion. |
| 285 | */ |
| 286 | struct ncsi_request *ncsi_alloc_request(struct ncsi_dev_priv *ndp, bool driven) |
| 287 | { |
| 288 | struct ncsi_request *nr = NULL; |
| 289 | int i, limit = ARRAY_SIZE(ndp->requests); |
| 290 | unsigned long flags; |
| 291 | |
| 292 | /* Check if there is one available request until the ceiling */ |
| 293 | spin_lock_irqsave(&ndp->lock, flags); |
| 294 | for (i = ndp->request_id; !nr && i < limit; i++) { |
| 295 | if (ndp->requests[i].used) |
| 296 | continue; |
| 297 | |
| 298 | nr = &ndp->requests[i]; |
| 299 | nr->used = true; |
| 300 | nr->driven = driven; |
| 301 | if (++ndp->request_id >= limit) |
| 302 | ndp->request_id = 0; |
| 303 | } |
| 304 | |
| 305 | /* Fail back to check from the starting cursor */ |
| 306 | for (i = 0; !nr && i < ndp->request_id; i++) { |
| 307 | if (ndp->requests[i].used) |
| 308 | continue; |
| 309 | |
| 310 | nr = &ndp->requests[i]; |
| 311 | nr->used = true; |
| 312 | nr->driven = driven; |
| 313 | if (++ndp->request_id >= limit) |
| 314 | ndp->request_id = 0; |
| 315 | } |
| 316 | spin_unlock_irqrestore(&ndp->lock, flags); |
| 317 | |
| 318 | return nr; |
| 319 | } |
| 320 | |
| 321 | void ncsi_free_request(struct ncsi_request *nr) |
| 322 | { |
| 323 | struct ncsi_dev_priv *ndp = nr->ndp; |
| 324 | struct sk_buff *cmd, *rsp; |
| 325 | unsigned long flags; |
| 326 | |
| 327 | if (nr->enabled) { |
| 328 | nr->enabled = false; |
| 329 | del_timer_sync(&nr->timer); |
| 330 | } |
| 331 | |
| 332 | spin_lock_irqsave(&ndp->lock, flags); |
| 333 | cmd = nr->cmd; |
| 334 | rsp = nr->rsp; |
| 335 | nr->cmd = NULL; |
| 336 | nr->rsp = NULL; |
| 337 | nr->used = false; |
| 338 | spin_unlock_irqrestore(&ndp->lock, flags); |
| 339 | |
| 340 | /* Release command and response */ |
| 341 | consume_skb(cmd); |
| 342 | consume_skb(rsp); |
| 343 | } |
| 344 | |
| 345 | struct ncsi_dev *ncsi_find_dev(struct net_device *dev) |
| 346 | { |
| 347 | struct ncsi_dev_priv *ndp; |
| 348 | |
| 349 | NCSI_FOR_EACH_DEV(ndp) { |
| 350 | if (ndp->ndev.dev == dev) |
| 351 | return &ndp->ndev; |
| 352 | } |
| 353 | |
| 354 | return NULL; |
| 355 | } |
| 356 | |
| 357 | static void ncsi_request_timeout(unsigned long data) |
| 358 | { |
| 359 | struct ncsi_request *nr = (struct ncsi_request *)data; |
| 360 | struct ncsi_dev_priv *ndp = nr->ndp; |
| 361 | unsigned long flags; |
| 362 | |
| 363 | /* If the request already had associated response, |
| 364 | * let the response handler to release it. |
| 365 | */ |
| 366 | spin_lock_irqsave(&ndp->lock, flags); |
| 367 | nr->enabled = false; |
| 368 | if (nr->rsp || !nr->cmd) { |
| 369 | spin_unlock_irqrestore(&ndp->lock, flags); |
| 370 | return; |
| 371 | } |
| 372 | spin_unlock_irqrestore(&ndp->lock, flags); |
| 373 | |
| 374 | /* Release the request */ |
| 375 | ncsi_free_request(nr); |
| 376 | } |
| 377 | |
| 378 | struct ncsi_dev *ncsi_register_dev(struct net_device *dev, |
| 379 | void (*handler)(struct ncsi_dev *ndev)) |
| 380 | { |
| 381 | struct ncsi_dev_priv *ndp; |
| 382 | struct ncsi_dev *nd; |
| 383 | unsigned long flags; |
| 384 | int i; |
| 385 | |
| 386 | /* Check if the device has been registered or not */ |
| 387 | nd = ncsi_find_dev(dev); |
| 388 | if (nd) |
| 389 | return nd; |
| 390 | |
| 391 | /* Create NCSI device */ |
| 392 | ndp = kzalloc(sizeof(*ndp), GFP_ATOMIC); |
| 393 | if (!ndp) |
| 394 | return NULL; |
| 395 | |
| 396 | nd = &ndp->ndev; |
| 397 | nd->state = ncsi_dev_state_registered; |
| 398 | nd->dev = dev; |
| 399 | nd->handler = handler; |
| 400 | |
| 401 | /* Initialize private NCSI device */ |
| 402 | spin_lock_init(&ndp->lock); |
| 403 | INIT_LIST_HEAD(&ndp->packages); |
| 404 | ndp->request_id = 0; |
| 405 | for (i = 0; i < ARRAY_SIZE(ndp->requests); i++) { |
| 406 | ndp->requests[i].id = i; |
| 407 | ndp->requests[i].ndp = ndp; |
| 408 | setup_timer(&ndp->requests[i].timer, |
| 409 | ncsi_request_timeout, |
| 410 | (unsigned long)&ndp->requests[i]); |
| 411 | } |
| 412 | |
| 413 | spin_lock_irqsave(&ncsi_dev_lock, flags); |
| 414 | list_add_tail_rcu(&ndp->node, &ncsi_dev_list); |
| 415 | spin_unlock_irqrestore(&ncsi_dev_lock, flags); |
| 416 | |
| 417 | return nd; |
| 418 | } |
| 419 | EXPORT_SYMBOL_GPL(ncsi_register_dev); |
| 420 | |
| 421 | void ncsi_unregister_dev(struct ncsi_dev *nd) |
| 422 | { |
| 423 | struct ncsi_dev_priv *ndp = TO_NCSI_DEV_PRIV(nd); |
| 424 | struct ncsi_package *np, *tmp; |
| 425 | unsigned long flags; |
| 426 | |
| 427 | list_for_each_entry_safe(np, tmp, &ndp->packages, node) |
| 428 | ncsi_remove_package(np); |
| 429 | |
| 430 | spin_lock_irqsave(&ncsi_dev_lock, flags); |
| 431 | list_del_rcu(&ndp->node); |
| 432 | spin_unlock_irqrestore(&ncsi_dev_lock, flags); |
| 433 | |
| 434 | kfree(ndp); |
| 435 | } |
| 436 | EXPORT_SYMBOL_GPL(ncsi_unregister_dev); |