drivers/rapidio/rio.c

   1 /*
   2  * RapidIO interconnect services
   3  * (RapidIO Interconnect Specification, http://www.rapidio.org)
   4  *
   5  * Copyright 2005 MontaVista Software, Inc.
   6  * Matt Porter <mporter@kernel.crashing.org>
   7  *
   8  * Copyright 2009 - 2013 Integrated Device Technology, Inc.
   9  * Alex Bounine <alexandre.bounine@idt.com>
  10  *
  11  * This program is free software; you can redistribute  it and/or modify it
  12  * under  the terms of  the GNU General  Public License as published by the
  13  * Free Software Foundation;  either version 2 of the  License, or (at your
  14  * option) any later version.
  15  */
  16
  17 #include <linux/types.h>
  18 #include <linux/kernel.h>
  19
  20 #include <linux/delay.h>
  21 #include <linux/init.h>
  22 #include <linux/rio.h>
  23 #include <linux/rio_drv.h>
  24 #include <linux/rio_ids.h>
  25 #include <linux/rio_regs.h>
  26 #include <linux/module.h>
  27 #include <linux/spinlock.h>
  28 #include <linux/slab.h>
  29 #include <linux/interrupt.h>
  30
  31 #include "rio.h"
  32
  33 MODULE_DESCRIPTION("RapidIO Subsystem Core");
  34 MODULE_AUTHOR("Matt Porter <mporter@kernel.crashing.org>");
  35 MODULE_AUTHOR("Alexandre Bounine <alexandre.bounine@idt.com>");
  36 MODULE_LICENSE("GPL");
  37
  38 static int hdid[RIO_MAX_MPORTS];
  39 static int ids_num;
  40 module_param_array(hdid, int, &ids_num, 0);
  41 MODULE_PARM_DESC(hdid,
  42         "Destination ID assignment to local RapidIO controllers");
  43
  44 static LIST_HEAD(rio_devices);
  45 static DEFINE_SPINLOCK(rio_global_list_lock);
  46
  47 static LIST_HEAD(rio_mports);
  48 static LIST_HEAD(rio_scans);
  49 static DEFINE_MUTEX(rio_mport_list_lock);
  50 static unsigned char next_portid;
  51 static DEFINE_SPINLOCK(rio_mmap_lock);
  52
  53 /**
  54  * rio_local_get_device_id - Get the base/extended device id for a port
  55  * @port: RIO master port from which to get the deviceid
  56  *
  57  * Reads the base/extended device id from the local device
  58  * implementing the master port. Returns the 8/16-bit device
  59  * id.
  60  */
  61 u16 rio_local_get_device_id(struct rio_mport *port)
  62 {
  63         u32 result;
  64
  65         rio_local_read_config_32(port, RIO_DID_CSR, &result);
  66
  67         return (RIO_GET_DID(port->sys_size, result));
  68 }
  69
  70 /**
  71  * rio_query_mport - Query mport device attributes
  72  * @port: mport device to query
  73  * @mport_attr: mport attributes data structure
  74  *
  75  * Returns attributes of specified mport through the
  76  * pointer to attributes data structure.
  77  */
  78 int rio_query_mport(struct rio_mport *port,
  79                     struct rio_mport_attr *mport_attr)
  80 {
  81         if (!port->ops->query_mport)
  82                 return -ENODATA;
  83         return port->ops->query_mport(port, mport_attr);
  84 }
  85 EXPORT_SYMBOL(rio_query_mport);
  86
  87 /**
  88  * rio_add_device- Adds a RIO device to the device model
  89  * @rdev: RIO device
  90  *
  91  * Adds the RIO device to the global device list and adds the RIO
  92  * device to the RIO device list.  Creates the generic sysfs nodes
  93  * for an RIO device.
  94  */
  95 int rio_add_device(struct rio_dev *rdev)
  96 {
  97         int err;
  98
  99         err = device_add(&rdev->dev);
 100         if (err)
 101                 return err;
 102
 103         spin_lock(&rio_global_list_lock);
 104         list_add_tail(&rdev->global_list, &rio_devices);
 105         spin_unlock(&rio_global_list_lock);
 106
 107         rio_create_sysfs_dev_files(rdev);
 108
 109         return 0;
 110 }
 111 EXPORT_SYMBOL_GPL(rio_add_device);
 112
 113 /**
 114  * rio_request_inb_mbox - request inbound mailbox service
 115  * @mport: RIO master port from which to allocate the mailbox resource
 116  * @dev_id: Device specific pointer to pass on event
 117  * @mbox: Mailbox number to claim
 118  * @entries: Number of entries in inbound mailbox queue
 119  * @minb: Callback to execute when inbound message is received
 120  *
 121  * Requests ownership of an inbound mailbox resource and binds
 122  * a callback function to the resource. Returns %0 on success.
 123  */
 124 int rio_request_inb_mbox(struct rio_mport *mport,
 125                          void *dev_id,
 126                          int mbox,
 127                          int entries,
 128                          void (*minb) (struct rio_mport * mport, void *dev_id, int mbox,
 129                                        int slot))
 130 {
 131         int rc = -ENOSYS;
 132         struct resource *res;
 133
 134         if (mport->ops->open_inb_mbox == NULL)
 135                 goto out;
 136
 137         res = kzalloc(sizeof(struct resource), GFP_KERNEL);
 138
 139         if (res) {
 140                 rio_init_mbox_res(res, mbox, mbox);
 141
 142                 /* Make sure this mailbox isn't in use */
 143                 if ((rc =
 144                      request_resource(&mport->riores[RIO_INB_MBOX_RESOURCE],
 145                                       res)) < 0) {
 146                         kfree(res);
 147                         goto out;
 148                 }
 149
 150                 mport->inb_msg[mbox].res = res;
 151
 152                 /* Hook the inbound message callback */
 153                 mport->inb_msg[mbox].mcback = minb;
 154
 155                 rc = mport->ops->open_inb_mbox(mport, dev_id, mbox, entries);
 156         } else
 157                 rc = -ENOMEM;
 158
 159       out:
 160         return rc;
 161 }
 162
 163 /**
 164  * rio_release_inb_mbox - release inbound mailbox message service
 165  * @mport: RIO master port from which to release the mailbox resource
 166  * @mbox: Mailbox number to release
 167  *
 168  * Releases ownership of an inbound mailbox resource. Returns 0
 169  * if the request has been satisfied.
 170  */
 171 int rio_release_inb_mbox(struct rio_mport *mport, int mbox)
 172 {
 173         if (mport->ops->close_inb_mbox) {
 174                 mport->ops->close_inb_mbox(mport, mbox);
 175
 176                 /* Release the mailbox resource */
 177                 return release_resource(mport->inb_msg[mbox].res);
 178         } else
 179                 return -ENOSYS;
 180 }
 181
 182 /**
 183  * rio_request_outb_mbox - request outbound mailbox service
 184  * @mport: RIO master port from which to allocate the mailbox resource
 185  * @dev_id: Device specific pointer to pass on event
 186  * @mbox: Mailbox number to claim
 187  * @entries: Number of entries in outbound mailbox queue
 188  * @moutb: Callback to execute when outbound message is sent
 189  *
 190  * Requests ownership of an outbound mailbox resource and binds
 191  * a callback function to the resource. Returns 0 on success.
 192  */
 193 int rio_request_outb_mbox(struct rio_mport *mport,
 194                           void *dev_id,
 195                           int mbox,
 196                           int entries,
 197                           void (*moutb) (struct rio_mport * mport, void *dev_id, int mbox, int slot))
 198 {
 199         int rc = -ENOSYS;
 200         struct resource *res;
 201
 202         if (mport->ops->open_outb_mbox == NULL)
 203                 goto out;
 204
 205         res = kzalloc(sizeof(struct resource), GFP_KERNEL);
 206
 207         if (res) {
 208                 rio_init_mbox_res(res, mbox, mbox);
 209
 210                 /* Make sure this outbound mailbox isn't in use */
 211                 if ((rc =
 212                      request_resource(&mport->riores[RIO_OUTB_MBOX_RESOURCE],
 213                                       res)) < 0) {
 214                         kfree(res);
 215                         goto out;
 216                 }
 217
 218                 mport->outb_msg[mbox].res = res;
 219
 220                 /* Hook the inbound message callback */
 221                 mport->outb_msg[mbox].mcback = moutb;
 222
 223                 rc = mport->ops->open_outb_mbox(mport, dev_id, mbox, entries);
 224         } else
 225                 rc = -ENOMEM;
 226
 227       out:
 228         return rc;
 229 }
 230
 231 /**
 232  * rio_release_outb_mbox - release outbound mailbox message service
 233  * @mport: RIO master port from which to release the mailbox resource
 234  * @mbox: Mailbox number to release
 235  *
 236  * Releases ownership of an inbound mailbox resource. Returns 0
 237  * if the request has been satisfied.
 238  */
 239 int rio_release_outb_mbox(struct rio_mport *mport, int mbox)
 240 {
 241         if (mport->ops->close_outb_mbox) {
 242                 mport->ops->close_outb_mbox(mport, mbox);
 243
 244                 /* Release the mailbox resource */
 245                 return release_resource(mport->outb_msg[mbox].res);
 246         } else
 247                 return -ENOSYS;
 248 }
 249
 250 /**
 251  * rio_setup_inb_dbell - bind inbound doorbell callback
 252  * @mport: RIO master port to bind the doorbell callback
 253  * @dev_id: Device specific pointer to pass on event
 254  * @res: Doorbell message resource
 255  * @dinb: Callback to execute when doorbell is received
 256  *
 257  * Adds a doorbell resource/callback pair into a port's
 258  * doorbell event list. Returns 0 if the request has been
 259  * satisfied.
 260  */
 261 static int
 262 rio_setup_inb_dbell(struct rio_mport *mport, void *dev_id, struct resource *res,
 263                     void (*dinb) (struct rio_mport * mport, void *dev_id, u16 src, u16 dst,
 264                                   u16 info))
 265 {
 266         int rc = 0;
 267         struct rio_dbell *dbell;
 268
 269         if (!(dbell = kmalloc(sizeof(struct rio_dbell), GFP_KERNEL))) {
 270                 rc = -ENOMEM;
 271                 goto out;
 272         }
 273
 274         dbell->res = res;
 275         dbell->dinb = dinb;
 276         dbell->dev_id = dev_id;
 277
 278         list_add_tail(&dbell->node, &mport->dbells);
 279
 280       out:
 281         return rc;
 282 }
 283
 284 /**
 285  * rio_request_inb_dbell - request inbound doorbell message service
 286  * @mport: RIO master port from which to allocate the doorbell resource
 287  * @dev_id: Device specific pointer to pass on event
 288  * @start: Doorbell info range start
 289  * @end: Doorbell info range end
 290  * @dinb: Callback to execute when doorbell is received
 291  *
 292  * Requests ownership of an inbound doorbell resource and binds
 293  * a callback function to the resource. Returns 0 if the request
 294  * has been satisfied.
 295  */
 296 int rio_request_inb_dbell(struct rio_mport *mport,
 297                           void *dev_id,
 298                           u16 start,
 299                           u16 end,
 300                           void (*dinb) (struct rio_mport * mport, void *dev_id, u16 src,
 301                                         u16 dst, u16 info))
 302 {
 303         int rc = 0;
 304
 305         struct resource *res = kzalloc(sizeof(struct resource), GFP_KERNEL);
 306
 307         if (res) {
 308                 rio_init_dbell_res(res, start, end);
 309
 310                 /* Make sure these doorbells aren't in use */
 311                 if ((rc =
 312                      request_resource(&mport->riores[RIO_DOORBELL_RESOURCE],
 313                                       res)) < 0) {
 314                         kfree(res);
 315                         goto out;
 316                 }
 317
 318                 /* Hook the doorbell callback */
 319                 rc = rio_setup_inb_dbell(mport, dev_id, res, dinb);
 320         } else
 321                 rc = -ENOMEM;
 322
 323       out:
 324         return rc;
 325 }
 326
 327 /**
 328  * rio_release_inb_dbell - release inbound doorbell message service
 329  * @mport: RIO master port from which to release the doorbell resource
 330  * @start: Doorbell info range start
 331  * @end: Doorbell info range end
 332  *
 333  * Releases ownership of an inbound doorbell resource and removes
 334  * callback from the doorbell event list. Returns 0 if the request
 335  * has been satisfied.
 336  */
 337 int rio_release_inb_dbell(struct rio_mport *mport, u16 start, u16 end)
 338 {
 339         int rc = 0, found = 0;
 340         struct rio_dbell *dbell;
 341
 342         list_for_each_entry(dbell, &mport->dbells, node) {
 343                 if ((dbell->res->start == start) && (dbell->res->end == end)) {
 344                         found = 1;
 345                         break;
 346                 }
 347         }
 348
 349         /* If we can't find an exact match, fail */
 350         if (!found) {
 351                 rc = -EINVAL;
 352                 goto out;
 353         }
 354
 355         /* Delete from list */
 356         list_del(&dbell->node);
 357
 358         /* Release the doorbell resource */
 359         rc = release_resource(dbell->res);
 360
 361         /* Free the doorbell event */
 362         kfree(dbell);
 363
 364       out:
 365         return rc;
 366 }
 367
 368 /**
 369  * rio_request_outb_dbell - request outbound doorbell message range
 370  * @rdev: RIO device from which to allocate the doorbell resource
 371  * @start: Doorbell message range start
 372  * @end: Doorbell message range end
 373  *
 374  * Requests ownership of a doorbell message range. Returns a resource
 375  * if the request has been satisfied or %NULL on failure.
 376  */
 377 struct resource *rio_request_outb_dbell(struct rio_dev *rdev, u16 start,
 378                                         u16 end)
 379 {
 380         struct resource *res = kzalloc(sizeof(struct resource), GFP_KERNEL);
 381
 382         if (res) {
 383                 rio_init_dbell_res(res, start, end);
 384
 385                 /* Make sure these doorbells aren't in use */
 386                 if (request_resource(&rdev->riores[RIO_DOORBELL_RESOURCE], res)
 387                     < 0) {
 388                         kfree(res);
 389                         res = NULL;
 390                 }
 391         }
 392
 393         return res;
 394 }
 395
 396 /**
 397  * rio_release_outb_dbell - release outbound doorbell message range
 398  * @rdev: RIO device from which to release the doorbell resource
 399  * @res: Doorbell resource to be freed
 400  *
 401  * Releases ownership of a doorbell message range. Returns 0 if the
 402  * request has been satisfied.
 403  */
 404 int rio_release_outb_dbell(struct rio_dev *rdev, struct resource *res)
 405 {
 406         int rc = release_resource(res);
 407
 408         kfree(res);
 409
 410         return rc;
 411 }
 412
 413 /**
 414  * rio_request_inb_pwrite - request inbound port-write message service
 415  * @rdev: RIO device to which register inbound port-write callback routine
 416  * @pwcback: Callback routine to execute when port-write is received
 417  *
 418  * Binds a port-write callback function to the RapidIO device.
 419  * Returns 0 if the request has been satisfied.
 420  */
 421 int rio_request_inb_pwrite(struct rio_dev *rdev,
 422         int (*pwcback)(struct rio_dev *rdev, union rio_pw_msg *msg, int step))
 423 {
 424         int rc = 0;
 425
 426         spin_lock(&rio_global_list_lock);
 427         if (rdev->pwcback != NULL)
 428                 rc = -ENOMEM;
 429         else
 430                 rdev->pwcback = pwcback;
 431
 432         spin_unlock(&rio_global_list_lock);
 433         return rc;
 434 }
 435 EXPORT_SYMBOL_GPL(rio_request_inb_pwrite);
 436
 437 /**
 438  * rio_release_inb_pwrite - release inbound port-write message service
 439  * @rdev: RIO device which registered for inbound port-write callback
 440  *
 441  * Removes callback from the rio_dev structure. Returns 0 if the request
 442  * has been satisfied.
 443  */
 444 int rio_release_inb_pwrite(struct rio_dev *rdev)
 445 {
 446         int rc = -ENOMEM;
 447
 448         spin_lock(&rio_global_list_lock);
 449         if (rdev->pwcback) {
 450                 rdev->pwcback = NULL;
 451                 rc = 0;
 452         }
 453
 454         spin_unlock(&rio_global_list_lock);
 455         return rc;
 456 }
 457 EXPORT_SYMBOL_GPL(rio_release_inb_pwrite);
 458
 459 /**
 460  * rio_map_inb_region -- Map inbound memory region.
 461  * @mport: Master port.
 462  * @local: physical address of memory region to be mapped
 463  * @rbase: RIO base address assigned to this window
 464  * @size: Size of the memory region
 465  * @rflags: Flags for mapping.
 466  *
 467  * Return: 0 -- Success.
 468  *
 469  * This function will create the mapping from RIO space to local memory.
 470  */
 471 int rio_map_inb_region(struct rio_mport *mport, dma_addr_t local,
 472                         u64 rbase, u32 size, u32 rflags)
 473 {
 474         int rc = 0;
 475         unsigned long flags;
 476
 477         if (!mport->ops->map_inb)
 478                 return -1;
 479         spin_lock_irqsave(&rio_mmap_lock, flags);
 480         rc = mport->ops->map_inb(mport, local, rbase, size, rflags);
 481         spin_unlock_irqrestore(&rio_mmap_lock, flags);
 482         return rc;
 483 }
 484 EXPORT_SYMBOL_GPL(rio_map_inb_region);
 485
 486 /**
 487  * rio_unmap_inb_region -- Unmap the inbound memory region
 488  * @mport: Master port
 489  * @lstart: physical address of memory region to be unmapped
 490  */
 491 void rio_unmap_inb_region(struct rio_mport *mport, dma_addr_t lstart)
 492 {
 493         unsigned long flags;
 494         if (!mport->ops->unmap_inb)
 495                 return;
 496         spin_lock_irqsave(&rio_mmap_lock, flags);
 497         mport->ops->unmap_inb(mport, lstart);
 498         spin_unlock_irqrestore(&rio_mmap_lock, flags);
 499 }
 500 EXPORT_SYMBOL_GPL(rio_unmap_inb_region);
 501
 502 /**
 503  * rio_mport_get_physefb - Helper function that returns register offset
 504  *                      for Physical Layer Extended Features Block.
 505  * @port: Master port to issue transaction
 506  * @local: Indicate a local master port or remote device access
 507  * @destid: Destination ID of the device
 508  * @hopcount: Number of switch hops to the device
 509  */
 510 u32
 511 rio_mport_get_physefb(struct rio_mport *port, int local,
 512                       u16 destid, u8 hopcount)
 513 {
 514         u32 ext_ftr_ptr;
 515         u32 ftr_header;
 516
 517         ext_ftr_ptr = rio_mport_get_efb(port, local, destid, hopcount, 0);
 518
 519         while (ext_ftr_ptr)  {
 520                 if (local)
 521                         rio_local_read_config_32(port, ext_ftr_ptr,
 522                                                  &ftr_header);
 523                 else
 524                         rio_mport_read_config_32(port, destid, hopcount,
 525                                                  ext_ftr_ptr, &ftr_header);
 526
 527                 ftr_header = RIO_GET_BLOCK_ID(ftr_header);
 528                 switch (ftr_header) {
 529
 530                 case RIO_EFB_SER_EP_ID_V13P:
 531                 case RIO_EFB_SER_EP_REC_ID_V13P:
 532                 case RIO_EFB_SER_EP_FREE_ID_V13P:
 533                 case RIO_EFB_SER_EP_ID:
 534                 case RIO_EFB_SER_EP_REC_ID:
 535                 case RIO_EFB_SER_EP_FREE_ID:
 536                 case RIO_EFB_SER_EP_FREC_ID:
 537
 538                         return ext_ftr_ptr;
 539
 540                 default:
 541                         break;
 542                 }
 543
 544                 ext_ftr_ptr = rio_mport_get_efb(port, local, destid,
 545                                                 hopcount, ext_ftr_ptr);
 546         }
 547
 548         return ext_ftr_ptr;
 549 }
 550 EXPORT_SYMBOL_GPL(rio_mport_get_physefb);
 551
 552 /**
 553  * rio_get_comptag - Begin or continue searching for a RIO device by component tag
 554  * @comp_tag: RIO component tag to match
 555  * @from: Previous RIO device found in search, or %NULL for new search
 556  *
 557  * Iterates through the list of known RIO devices. If a RIO device is
 558  * found with a matching @comp_tag, a pointer to its device
 559  * structure is returned. Otherwise, %NULL is returned. A new search
 560  * is initiated by passing %NULL to the @from argument. Otherwise, if
 561  * @from is not %NULL, searches continue from next device on the global
 562  * list.
 563  */
 564 struct rio_dev *rio_get_comptag(u32 comp_tag, struct rio_dev *from)
 565 {
 566         struct list_head *n;
 567         struct rio_dev *rdev;
 568
 569         spin_lock(&rio_global_list_lock);
 570         n = from ? from->global_list.next : rio_devices.next;
 571
 572         while (n && (n != &rio_devices)) {
 573                 rdev = rio_dev_g(n);
 574                 if (rdev->comp_tag == comp_tag)
 575                         goto exit;
 576                 n = n->next;
 577         }
 578         rdev = NULL;
 579 exit:
 580         spin_unlock(&rio_global_list_lock);
 581         return rdev;
 582 }
 583 EXPORT_SYMBOL_GPL(rio_get_comptag);
 584
 585 /**
 586  * rio_set_port_lockout - Sets/clears LOCKOUT bit (RIO EM 1.3) for a switch port.
 587  * @rdev: Pointer to RIO device control structure
 588  * @pnum: Switch port number to set LOCKOUT bit
 589  * @lock: Operation : set (=1) or clear (=0)
 590  */
 591 int rio_set_port_lockout(struct rio_dev *rdev, u32 pnum, int lock)
 592 {
 593         u32 regval;
 594
 595         rio_read_config_32(rdev,
 596                                  rdev->phys_efptr + RIO_PORT_N_CTL_CSR(pnum),
 597                                  &regval);
 598         if (lock)
 599                 regval |= RIO_PORT_N_CTL_LOCKOUT;
 600         else
 601                 regval &= ~RIO_PORT_N_CTL_LOCKOUT;
 602
 603         rio_write_config_32(rdev,
 604                                   rdev->phys_efptr + RIO_PORT_N_CTL_CSR(pnum),
 605                                   regval);
 606         return 0;
 607 }
 608 EXPORT_SYMBOL_GPL(rio_set_port_lockout);
 609
 610 /**
 611  * rio_enable_rx_tx_port - enable input receiver and output transmitter of
 612  * given port
 613  * @port: Master port associated with the RIO network
 614  * @local: local=1 select local port otherwise a far device is reached
 615  * @destid: Destination ID of the device to check host bit
 616  * @hopcount: Number of hops to reach the target
 617  * @port_num: Port (-number on switch) to enable on a far end device
 618  *
 619  * Returns 0 or 1 from on General Control Command and Status Register
 620  * (EXT_PTR+0x3C)
 621  */
 622 int rio_enable_rx_tx_port(struct rio_mport *port,
 623                           int local, u16 destid,
 624                           u8 hopcount, u8 port_num)
 625 {
 626 #ifdef CONFIG_RAPIDIO_ENABLE_RX_TX_PORTS
 627         u32 regval;
 628         u32 ext_ftr_ptr;
 629
 630         /*
 631         * enable rx input tx output port
 632         */
 633         pr_debug("rio_enable_rx_tx_port(local = %d, destid = %d, hopcount = "
 634                  "%d, port_num = %d)\n", local, destid, hopcount, port_num);
 635
 636         ext_ftr_ptr = rio_mport_get_physefb(port, local, destid, hopcount);
 637
 638         if (local) {
 639                 rio_local_read_config_32(port, ext_ftr_ptr +
 640                                 RIO_PORT_N_CTL_CSR(0),
 641                                 &regval);
 642         } else {
 643                 if (rio_mport_read_config_32(port, destid, hopcount,
 644                 ext_ftr_ptr + RIO_PORT_N_CTL_CSR(port_num), &regval) < 0)
 645                         return -EIO;
 646         }
 647
 648         if (regval & RIO_PORT_N_CTL_P_TYP_SER) {
 649                 /* serial */
 650                 regval = regval | RIO_PORT_N_CTL_EN_RX_SER
 651                                 | RIO_PORT_N_CTL_EN_TX_SER;
 652         } else {
 653                 /* parallel */
 654                 regval = regval | RIO_PORT_N_CTL_EN_RX_PAR
 655                                 | RIO_PORT_N_CTL_EN_TX_PAR;
 656         }
 657
 658         if (local) {
 659                 rio_local_write_config_32(port, ext_ftr_ptr +
 660                                           RIO_PORT_N_CTL_CSR(0), regval);
 661         } else {
 662                 if (rio_mport_write_config_32(port, destid, hopcount,
 663                     ext_ftr_ptr + RIO_PORT_N_CTL_CSR(port_num), regval) < 0)
 664                         return -EIO;
 665         }
 666 #endif
 667         return 0;
 668 }
 669 EXPORT_SYMBOL_GPL(rio_enable_rx_tx_port);
 670
 671
 672 /**
 673  * rio_chk_dev_route - Validate route to the specified device.
 674  * @rdev:  RIO device failed to respond
 675  * @nrdev: Last active device on the route to rdev
 676  * @npnum: nrdev's port number on the route to rdev
 677  *
 678  * Follows a route to the specified RIO device to determine the last available
 679  * device (and corresponding RIO port) on the route.
 680  */
 681 static int
 682 rio_chk_dev_route(struct rio_dev *rdev, struct rio_dev **nrdev, int *npnum)
 683 {
 684         u32 result;
 685         int p_port, rc = -EIO;
 686         struct rio_dev *prev = NULL;
 687
 688         /* Find switch with failed RIO link */
 689         while (rdev->prev && (rdev->prev->pef & RIO_PEF_SWITCH)) {
 690                 if (!rio_read_config_32(rdev->prev, RIO_DEV_ID_CAR, &result)) {
 691                         prev = rdev->prev;
 692                         break;
 693                 }
 694                 rdev = rdev->prev;
 695         }
 696
 697         if (prev == NULL)
 698                 goto err_out;
 699
 700         p_port = prev->rswitch->route_table[rdev->destid];
 701
 702         if (p_port != RIO_INVALID_ROUTE) {
 703                 pr_debug("RIO: link failed on [%s]-P%d\n",
 704                          rio_name(prev), p_port);
 705                 *nrdev = prev;
 706                 *npnum = p_port;
 707                 rc = 0;
 708         } else
 709                 pr_debug("RIO: failed to trace route to %s\n", rio_name(rdev));
 710 err_out:
 711         return rc;
 712 }
 713
 714 /**
 715  * rio_mport_chk_dev_access - Validate access to the specified device.
 716  * @mport: Master port to send transactions
 717  * @destid: Device destination ID in network
 718  * @hopcount: Number of hops into the network
 719  */
 720 int
 721 rio_mport_chk_dev_access(struct rio_mport *mport, u16 destid, u8 hopcount)
 722 {
 723         int i = 0;
 724         u32 tmp;
 725
 726         while (rio_mport_read_config_32(mport, destid, hopcount,
 727                                         RIO_DEV_ID_CAR, &tmp)) {
 728                 i++;
 729                 if (i == RIO_MAX_CHK_RETRY)
 730                         return -EIO;
 731                 mdelay(1);
 732         }
 733
 734         return 0;
 735 }
 736 EXPORT_SYMBOL_GPL(rio_mport_chk_dev_access);
 737
 738 /**
 739  * rio_chk_dev_access - Validate access to the specified device.
 740  * @rdev: Pointer to RIO device control structure
 741  */
 742 static int rio_chk_dev_access(struct rio_dev *rdev)
 743 {
 744         return rio_mport_chk_dev_access(rdev->net->hport,
 745                                         rdev->destid, rdev->hopcount);
 746 }
 747
 748 /**
 749  * rio_get_input_status - Sends a Link-Request/Input-Status control symbol and
 750  *                        returns link-response (if requested).
 751  * @rdev: RIO devive to issue Input-status command
 752  * @pnum: Device port number to issue the command
 753  * @lnkresp: Response from a link partner
 754  */
 755 static int
 756 rio_get_input_status(struct rio_dev *rdev, int pnum, u32 *lnkresp)
 757 {
 758         u32 regval;
 759         int checkcount;
 760
 761         if (lnkresp) {
 762                 /* Read from link maintenance response register
 763                  * to clear valid bit */
 764                 rio_read_config_32(rdev,
 765                         rdev->phys_efptr + RIO_PORT_N_MNT_RSP_CSR(pnum),
 766                         &regval);
 767                 udelay(50);
 768         }
 769
 770         /* Issue Input-status command */
 771         rio_write_config_32(rdev,
 772                 rdev->phys_efptr + RIO_PORT_N_MNT_REQ_CSR(pnum),
 773                 RIO_MNT_REQ_CMD_IS);
 774
 775         /* Exit if the response is not expected */
 776         if (lnkresp == NULL)
 777                 return 0;
 778
 779         checkcount = 3;
 780         while (checkcount--) {
 781                 udelay(50);
 782                 rio_read_config_32(rdev,
 783                         rdev->phys_efptr + RIO_PORT_N_MNT_RSP_CSR(pnum),
 784                         &regval);
 785                 if (regval & RIO_PORT_N_MNT_RSP_RVAL) {
 786                         *lnkresp = regval;
 787                         return 0;
 788                 }
 789         }
 790
 791         return -EIO;
 792 }
 793
 794 /**
 795  * rio_clr_err_stopped - Clears port Error-stopped states.
 796  * @rdev: Pointer to RIO device control structure
 797  * @pnum: Switch port number to clear errors
 798  * @err_status: port error status (if 0 reads register from device)
 799  */
 800 static int rio_clr_err_stopped(struct rio_dev *rdev, u32 pnum, u32 err_status)
 801 {
 802         struct rio_dev *nextdev = rdev->rswitch->nextdev[pnum];
 803         u32 regval;
 804         u32 far_ackid, far_linkstat, near_ackid;
 805
 806         if (err_status == 0)
 807                 rio_read_config_32(rdev,
 808                         rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(pnum),
 809                         &err_status);
 810
 811         if (err_status & RIO_PORT_N_ERR_STS_PW_OUT_ES) {
 812                 pr_debug("RIO_EM: servicing Output Error-Stopped state\n");
 813                 /*
 814                  * Send a Link-Request/Input-Status control symbol
 815                  */
 816                 if (rio_get_input_status(rdev, pnum, &regval)) {
 817                         pr_debug("RIO_EM: Input-status response timeout\n");
 818                         goto rd_err;
 819                 }
 820
 821                 pr_debug("RIO_EM: SP%d Input-status response=0x%08x\n",
 822                          pnum, regval);
 823                 far_ackid = (regval & RIO_PORT_N_MNT_RSP_ASTAT) >> 5;
 824                 far_linkstat = regval & RIO_PORT_N_MNT_RSP_LSTAT;
 825                 rio_read_config_32(rdev,
 826                         rdev->phys_efptr + RIO_PORT_N_ACK_STS_CSR(pnum),
 827                         &regval);
 828                 pr_debug("RIO_EM: SP%d_ACK_STS_CSR=0x%08x\n", pnum, regval);
 829                 near_ackid = (regval & RIO_PORT_N_ACK_INBOUND) >> 24;
 830                 pr_debug("RIO_EM: SP%d far_ackID=0x%02x far_linkstat=0x%02x" \
 831                          " near_ackID=0x%02x\n",
 832                         pnum, far_ackid, far_linkstat, near_ackid);
 833
 834                 /*
 835                  * If required, synchronize ackIDs of near and
 836                  * far sides.
 837                  */
 838                 if ((far_ackid != ((regval & RIO_PORT_N_ACK_OUTSTAND) >> 8)) ||
 839                     (far_ackid != (regval & RIO_PORT_N_ACK_OUTBOUND))) {
 840                         /* Align near outstanding/outbound ackIDs with
 841                          * far inbound.
 842                          */
 843                         rio_write_config_32(rdev,
 844                                 rdev->phys_efptr + RIO_PORT_N_ACK_STS_CSR(pnum),
 845                                 (near_ackid << 24) |
 846                                         (far_ackid << 8) | far_ackid);
 847                         /* Align far outstanding/outbound ackIDs with
 848                          * near inbound.
 849                          */
 850                         far_ackid++;
 851                         if (nextdev)
 852                                 rio_write_config_32(nextdev,
 853                                         nextdev->phys_efptr +
 854                                         RIO_PORT_N_ACK_STS_CSR(RIO_GET_PORT_NUM(nextdev->swpinfo)),
 855                                         (far_ackid << 24) |
 856                                         (near_ackid << 8) | near_ackid);
 857                         else
 858                                 pr_debug("RIO_EM: Invalid nextdev pointer (NULL)\n");
 859                 }
 860 rd_err:
 861                 rio_read_config_32(rdev,
 862                         rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(pnum),
 863                         &err_status);
 864                 pr_debug("RIO_EM: SP%d_ERR_STS_CSR=0x%08x\n", pnum, err_status);
 865         }
 866
 867         if ((err_status & RIO_PORT_N_ERR_STS_PW_INP_ES) && nextdev) {
 868                 pr_debug("RIO_EM: servicing Input Error-Stopped state\n");
 869                 rio_get_input_status(nextdev,
 870                                      RIO_GET_PORT_NUM(nextdev->swpinfo), NULL);
 871                 udelay(50);
 872
 873                 rio_read_config_32(rdev,
 874                         rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(pnum),
 875                         &err_status);
 876                 pr_debug("RIO_EM: SP%d_ERR_STS_CSR=0x%08x\n", pnum, err_status);
 877         }
 878
 879         return (err_status & (RIO_PORT_N_ERR_STS_PW_OUT_ES |
 880                               RIO_PORT_N_ERR_STS_PW_INP_ES)) ? 1 : 0;
 881 }
 882
 883 /**
 884  * rio_inb_pwrite_handler - process inbound port-write message
 885  * @pw_msg: pointer to inbound port-write message
 886  *
 887  * Processes an inbound port-write message. Returns 0 if the request
 888  * has been satisfied.
 889  */
 890 int rio_inb_pwrite_handler(union rio_pw_msg *pw_msg)
 891 {
 892         struct rio_dev *rdev;
 893         u32 err_status, em_perrdet, em_ltlerrdet;
 894         int rc, portnum;
 895
 896         rdev = rio_get_comptag((pw_msg->em.comptag & RIO_CTAG_UDEVID), NULL);
 897         if (rdev == NULL) {
 898                 /* Device removed or enumeration error */
 899                 pr_debug("RIO: %s No matching device for CTag 0x%08x\n",
 900                         __func__, pw_msg->em.comptag);
 901                 return -EIO;
 902         }
 903
 904         pr_debug("RIO: Port-Write message from %s\n", rio_name(rdev));
 905
 906 #ifdef DEBUG_PW
 907         {
 908         u32 i;
 909         for (i = 0; i < RIO_PW_MSG_SIZE/sizeof(u32);) {
 910                         pr_debug("0x%02x: %08x %08x %08x %08x\n",
 911                                  i*4, pw_msg->raw[i], pw_msg->raw[i + 1],
 912                                  pw_msg->raw[i + 2], pw_msg->raw[i + 3]);
 913                         i += 4;
 914         }
 915         }
 916 #endif
 917
 918         /* Call an external service function (if such is registered
 919          * for this device). This may be the service for endpoints that send
 920          * device-specific port-write messages. End-point messages expected
 921          * to be handled completely by EP specific device driver.
 922          * For switches rc==0 signals that no standard processing required.
 923          */
 924         if (rdev->pwcback != NULL) {
 925                 rc = rdev->pwcback(rdev, pw_msg, 0);
 926                 if (rc == 0)
 927                         return 0;
 928         }
 929
 930         portnum = pw_msg->em.is_port & 0xFF;
 931
 932         /* Check if device and route to it are functional:
 933          * Sometimes devices may send PW message(s) just before being
 934          * powered down (or link being lost).
 935          */
 936         if (rio_chk_dev_access(rdev)) {
 937                 pr_debug("RIO: device access failed - get link partner\n");
 938                 /* Scan route to the device and identify failed link.
 939                  * This will replace device and port reported in PW message.
 940                  * PW message should not be used after this point.
 941                  */
 942                 if (rio_chk_dev_route(rdev, &rdev, &portnum)) {
 943                         pr_err("RIO: Route trace for %s failed\n",
 944                                 rio_name(rdev));
 945                         return -EIO;
 946                 }
 947                 pw_msg = NULL;
 948         }
 949
 950         /* For End-point devices processing stops here */
 951         if (!(rdev->pef & RIO_PEF_SWITCH))
 952                 return 0;
 953
 954         if (rdev->phys_efptr == 0) {
 955                 pr_err("RIO_PW: Bad switch initialization for %s\n",
 956                         rio_name(rdev));
 957                 return 0;
 958         }
 959
 960         /*
 961          * Process the port-write notification from switch
 962          */
 963         if (rdev->rswitch->ops && rdev->rswitch->ops->em_handle)
 964                 rdev->rswitch->ops->em_handle(rdev, portnum);
 965
 966         rio_read_config_32(rdev,
 967                         rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(portnum),
 968                         &err_status);
 969         pr_debug("RIO_PW: SP%d_ERR_STS_CSR=0x%08x\n", portnum, err_status);
 970
 971         if (err_status & RIO_PORT_N_ERR_STS_PORT_OK) {
 972
 973                 if (!(rdev->rswitch->port_ok & (1 << portnum))) {
 974                         rdev->rswitch->port_ok |= (1 << portnum);
 975                         rio_set_port_lockout(rdev, portnum, 0);
 976                         /* Schedule Insertion Service */
 977                         pr_debug("RIO_PW: Device Insertion on [%s]-P%d\n",
 978                                rio_name(rdev), portnum);
 979                 }
 980
 981                 /* Clear error-stopped states (if reported).
 982                  * Depending on the link partner state, two attempts
 983                  * may be needed for successful recovery.
 984                  */
 985                 if (err_status & (RIO_PORT_N_ERR_STS_PW_OUT_ES |
 986                                   RIO_PORT_N_ERR_STS_PW_INP_ES)) {
 987                         if (rio_clr_err_stopped(rdev, portnum, err_status))
 988                                 rio_clr_err_stopped(rdev, portnum, 0);
 989                 }
 990         }  else { /* if (err_status & RIO_PORT_N_ERR_STS_PORT_UNINIT) */
 991
 992                 if (rdev->rswitch->port_ok & (1 << portnum)) {
 993                         rdev->rswitch->port_ok &= ~(1 << portnum);
 994                         rio_set_port_lockout(rdev, portnum, 1);
 995
 996                         rio_write_config_32(rdev,
 997                                 rdev->phys_efptr +
 998                                         RIO_PORT_N_ACK_STS_CSR(portnum),
 999                                 RIO_PORT_N_ACK_CLEAR);
1000
1001                         /* Schedule Extraction Service */
1002                         pr_debug("RIO_PW: Device Extraction on [%s]-P%d\n",
1003                                rio_name(rdev), portnum);
1004                 }
1005         }
1006
1007         rio_read_config_32(rdev,
1008                 rdev->em_efptr + RIO_EM_PN_ERR_DETECT(portnum), &em_perrdet);
1009         if (em_perrdet) {
1010                 pr_debug("RIO_PW: RIO_EM_P%d_ERR_DETECT=0x%08x\n",
1011                          portnum, em_perrdet);
1012                 /* Clear EM Port N Error Detect CSR */
1013                 rio_write_config_32(rdev,
1014                         rdev->em_efptr + RIO_EM_PN_ERR_DETECT(portnum), 0);
1015         }
1016
1017         rio_read_config_32(rdev,
1018                 rdev->em_efptr + RIO_EM_LTL_ERR_DETECT, &em_ltlerrdet);
1019         if (em_ltlerrdet) {
1020                 pr_debug("RIO_PW: RIO_EM_LTL_ERR_DETECT=0x%08x\n",
1021                          em_ltlerrdet);
1022                 /* Clear EM L/T Layer Error Detect CSR */
1023                 rio_write_config_32(rdev,
1024                         rdev->em_efptr + RIO_EM_LTL_ERR_DETECT, 0);
1025         }
1026
1027         /* Clear remaining error bits and Port-Write Pending bit */
1028         rio_write_config_32(rdev,
1029                         rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(portnum),
1030                         err_status);
1031
1032         return 0;
1033 }
1034 EXPORT_SYMBOL_GPL(rio_inb_pwrite_handler);
1035
1036 /**
1037  * rio_mport_get_efb - get pointer to next extended features block
1038  * @port: Master port to issue transaction
1039  * @local: Indicate a local master port or remote device access
1040  * @destid: Destination ID of the device
1041  * @hopcount: Number of switch hops to the device
1042  * @from: Offset of  current Extended Feature block header (if 0 starts
1043  * from ExtFeaturePtr)
1044  */
1045 u32
1046 rio_mport_get_efb(struct rio_mport *port, int local, u16 destid,
1047                       u8 hopcount, u32 from)
1048 {
1049         u32 reg_val;
1050
1051         if (from == 0) {
1052                 if (local)
1053                         rio_local_read_config_32(port, RIO_ASM_INFO_CAR,
1054                                                  &reg_val);
1055                 else
1056                         rio_mport_read_config_32(port, destid, hopcount,
1057                                                  RIO_ASM_INFO_CAR, &reg_val);
1058                 return reg_val & RIO_EXT_FTR_PTR_MASK;
1059         } else {
1060                 if (local)
1061                         rio_local_read_config_32(port, from, &reg_val);
1062                 else
1063                         rio_mport_read_config_32(port, destid, hopcount,
1064                                                  from, &reg_val);
1065                 return RIO_GET_BLOCK_ID(reg_val);
1066         }
1067 }
1068 EXPORT_SYMBOL_GPL(rio_mport_get_efb);
1069
1070 /**
1071  * rio_mport_get_feature - query for devices' extended features
1072  * @port: Master port to issue transaction
1073  * @local: Indicate a local master port or remote device access
1074  * @destid: Destination ID of the device
1075  * @hopcount: Number of switch hops to the device
1076  * @ftr: Extended feature code
1077  *
1078  * Tell if a device supports a given RapidIO capability.
1079  * Returns the offset of the requested extended feature
1080  * block within the device's RIO configuration space or
1081  * 0 in case the device does not support it.  Possible
1082  * values for @ftr:
1083  *
1084  * %RIO_EFB_PAR_EP_ID           LP/LVDS EP Devices
1085  *
1086  * %RIO_EFB_PAR_EP_REC_ID       LP/LVDS EP Recovery Devices
1087  *
1088  * %RIO_EFB_PAR_EP_FREE_ID      LP/LVDS EP Free Devices
1089  *
1090  * %RIO_EFB_SER_EP_ID           LP/Serial EP Devices
1091  *
1092  * %RIO_EFB_SER_EP_REC_ID       LP/Serial EP Recovery Devices
1093  *
1094  * %RIO_EFB_SER_EP_FREE_ID      LP/Serial EP Free Devices
1095  */
1096 u32
1097 rio_mport_get_feature(struct rio_mport * port, int local, u16 destid,
1098                       u8 hopcount, int ftr)
1099 {
1100         u32 asm_info, ext_ftr_ptr, ftr_header;
1101
1102         if (local)
1103                 rio_local_read_config_32(port, RIO_ASM_INFO_CAR, &asm_info);
1104         else
1105                 rio_mport_read_config_32(port, destid, hopcount,
1106                                          RIO_ASM_INFO_CAR, &asm_info);
1107
1108         ext_ftr_ptr = asm_info & RIO_EXT_FTR_PTR_MASK;
1109
1110         while (ext_ftr_ptr) {
1111                 if (local)
1112                         rio_local_read_config_32(port, ext_ftr_ptr,
1113                                                  &ftr_header);
1114                 else
1115                         rio_mport_read_config_32(port, destid, hopcount,
1116                                                  ext_ftr_ptr, &ftr_header);
1117                 if (RIO_GET_BLOCK_ID(ftr_header) == ftr)
1118                         return ext_ftr_ptr;
1119                 if (!(ext_ftr_ptr = RIO_GET_BLOCK_PTR(ftr_header)))
1120                         break;
1121         }
1122
1123         return 0;
1124 }
1125 EXPORT_SYMBOL_GPL(rio_mport_get_feature);
1126
1127 /**
1128  * rio_get_asm - Begin or continue searching for a RIO device by vid/did/asm_vid/asm_did
1129  * @vid: RIO vid to match or %RIO_ANY_ID to match all vids
1130  * @did: RIO did to match or %RIO_ANY_ID to match all dids
1131  * @asm_vid: RIO asm_vid to match or %RIO_ANY_ID to match all asm_vids
1132  * @asm_did: RIO asm_did to match or %RIO_ANY_ID to match all asm_dids
1133  * @from: Previous RIO device found in search, or %NULL for new search
1134  *
1135  * Iterates through the list of known RIO devices. If a RIO device is
1136  * found with a matching @vid, @did, @asm_vid, @asm_did, the reference
1137  * count to the device is incrememted and a pointer to its device
1138  * structure is returned. Otherwise, %NULL is returned. A new search
1139  * is initiated by passing %NULL to the @from argument. Otherwise, if
1140  * @from is not %NULL, searches continue from next device on the global
1141  * list. The reference count for @from is always decremented if it is
1142  * not %NULL.
1143  */
1144 struct rio_dev *rio_get_asm(u16 vid, u16 did,
1145                             u16 asm_vid, u16 asm_did, struct rio_dev *from)
1146 {
1147         struct list_head *n;
1148         struct rio_dev *rdev;
1149
1150         WARN_ON(in_interrupt());
1151         spin_lock(&rio_global_list_lock);
1152         n = from ? from->global_list.next : rio_devices.next;
1153
1154         while (n && (n != &rio_devices)) {
1155                 rdev = rio_dev_g(n);
1156                 if ((vid == RIO_ANY_ID || rdev->vid == vid) &&
1157                     (did == RIO_ANY_ID || rdev->did == did) &&
1158                     (asm_vid == RIO_ANY_ID || rdev->asm_vid == asm_vid) &&
1159                     (asm_did == RIO_ANY_ID || rdev->asm_did == asm_did))
1160                         goto exit;
1161                 n = n->next;
1162         }
1163         rdev = NULL;
1164       exit:
1165         rio_dev_put(from);
1166         rdev = rio_dev_get(rdev);
1167         spin_unlock(&rio_global_list_lock);
1168         return rdev;
1169 }
1170
1171 /**
1172  * rio_get_device - Begin or continue searching for a RIO device by vid/did
1173  * @vid: RIO vid to match or %RIO_ANY_ID to match all vids
1174  * @did: RIO did to match or %RIO_ANY_ID to match all dids
1175  * @from: Previous RIO device found in search, or %NULL for new search
1176  *
1177  * Iterates through the list of known RIO devices. If a RIO device is
1178  * found with a matching @vid and @did, the reference count to the
1179  * device is incrememted and a pointer to its device structure is returned.
1180  * Otherwise, %NULL is returned. A new search is initiated by passing %NULL
1181  * to the @from argument. Otherwise, if @from is not %NULL, searches
1182  * continue from next device on the global list. The reference count for
1183  * @from is always decremented if it is not %NULL.
1184  */
1185 struct rio_dev *rio_get_device(u16 vid, u16 did, struct rio_dev *from)
1186 {
1187         return rio_get_asm(vid, did, RIO_ANY_ID, RIO_ANY_ID, from);
1188 }
1189
1190 /**
1191  * rio_std_route_add_entry - Add switch route table entry using standard
1192  *   registers defined in RIO specification rev.1.3
1193  * @mport: Master port to issue transaction
1194  * @destid: Destination ID of the device
1195  * @hopcount: Number of switch hops to the device
1196  * @table: routing table ID (global or port-specific)
1197  * @route_destid: destID entry in the RT
1198  * @route_port: destination port for specified destID
1199  */
1200 static int
1201 rio_std_route_add_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
1202                         u16 table, u16 route_destid, u8 route_port)
1203 {
1204         if (table == RIO_GLOBAL_TABLE) {
1205                 rio_mport_write_config_32(mport, destid, hopcount,
1206                                 RIO_STD_RTE_CONF_DESTID_SEL_CSR,
1207                                 (u32)route_destid);
1208                 rio_mport_write_config_32(mport, destid, hopcount,
1209                                 RIO_STD_RTE_CONF_PORT_SEL_CSR,
1210                                 (u32)route_port);
1211         }
1212
1213         udelay(10);
1214         return 0;
1215 }
1216
1217 /**
1218  * rio_std_route_get_entry - Read switch route table entry (port number)
1219  *   associated with specified destID using standard registers defined in RIO
1220  *   specification rev.1.3
1221  * @mport: Master port to issue transaction
1222  * @destid: Destination ID of the device
1223  * @hopcount: Number of switch hops to the device
1224  * @table: routing table ID (global or port-specific)
1225  * @route_destid: destID entry in the RT
1226  * @route_port: returned destination port for specified destID
1227  */
1228 static int
1229 rio_std_route_get_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
1230                         u16 table, u16 route_destid, u8 *route_port)
1231 {
1232         u32 result;
1233
1234         if (table == RIO_GLOBAL_TABLE) {
1235                 rio_mport_write_config_32(mport, destid, hopcount,
1236                                 RIO_STD_RTE_CONF_DESTID_SEL_CSR, route_destid);
1237                 rio_mport_read_config_32(mport, destid, hopcount,
1238                                 RIO_STD_RTE_CONF_PORT_SEL_CSR, &result);
1239
1240                 *route_port = (u8)result;
1241         }
1242
1243         return 0;
1244 }
1245
1246 /**
1247  * rio_std_route_clr_table - Clear swotch route table using standard registers
1248  *   defined in RIO specification rev.1.3.
1249  * @mport: Master port to issue transaction
1250  * @destid: Destination ID of the device
1251  * @hopcount: Number of switch hops to the device
1252  * @table: routing table ID (global or port-specific)
1253  */
1254 static int
1255 rio_std_route_clr_table(struct rio_mport *mport, u16 destid, u8 hopcount,
1256                         u16 table)
1257 {
1258         u32 max_destid = 0xff;
1259         u32 i, pef, id_inc = 1, ext_cfg = 0;
1260         u32 port_sel = RIO_INVALID_ROUTE;
1261
1262         if (table == RIO_GLOBAL_TABLE) {
1263                 rio_mport_read_config_32(mport, destid, hopcount,
1264                                          RIO_PEF_CAR, &pef);
1265
1266                 if (mport->sys_size) {
1267                         rio_mport_read_config_32(mport, destid, hopcount,
1268                                                  RIO_SWITCH_RT_LIMIT,
1269                                                  &max_destid);
1270                         max_destid &= RIO_RT_MAX_DESTID;
1271                 }
1272
1273                 if (pef & RIO_PEF_EXT_RT) {
1274                         ext_cfg = 0x80000000;
1275                         id_inc = 4;
1276                         port_sel = (RIO_INVALID_ROUTE << 24) |
1277                                    (RIO_INVALID_ROUTE << 16) |
1278                                    (RIO_INVALID_ROUTE << 8) |
1279                                    RIO_INVALID_ROUTE;
1280                 }
1281
1282                 for (i = 0; i <= max_destid;) {
1283                         rio_mport_write_config_32(mport, destid, hopcount,
1284                                         RIO_STD_RTE_CONF_DESTID_SEL_CSR,
1285                                         ext_cfg | i);
1286                         rio_mport_write_config_32(mport, destid, hopcount,
1287                                         RIO_STD_RTE_CONF_PORT_SEL_CSR,
1288                                         port_sel);
1289                         i += id_inc;
1290                 }
1291         }
1292
1293         udelay(10);
1294         return 0;
1295 }
1296
1297 /**
1298  * rio_lock_device - Acquires host device lock for specified device
1299  * @port: Master port to send transaction
1300  * @destid: Destination ID for device/switch
1301  * @hopcount: Hopcount to reach switch
1302  * @wait_ms: Max wait time in msec (0 = no timeout)
1303  *
1304  * Attepts to acquire host device lock for specified device
1305  * Returns 0 if device lock acquired or EINVAL if timeout expires.
1306  */
1307 int rio_lock_device(struct rio_mport *port, u16 destid,
1308                     u8 hopcount, int wait_ms)
1309 {
1310         u32 result;
1311         int tcnt = 0;
1312
1313         /* Attempt to acquire device lock */
1314         rio_mport_write_config_32(port, destid, hopcount,
1315                                   RIO_HOST_DID_LOCK_CSR, port->host_deviceid);
1316         rio_mport_read_config_32(port, destid, hopcount,
1317                                  RIO_HOST_DID_LOCK_CSR, &result);
1318
1319         while (result != port->host_deviceid) {
1320                 if (wait_ms != 0 && tcnt == wait_ms) {
1321                         pr_debug("RIO: timeout when locking device %x:%x\n",
1322                                 destid, hopcount);
1323                         return -EINVAL;
1324                 }
1325
1326                 /* Delay a bit */
1327                 mdelay(1);
1328                 tcnt++;
1329                 /* Try to acquire device lock again */
1330                 rio_mport_write_config_32(port, destid,
1331                         hopcount,
1332                         RIO_HOST_DID_LOCK_CSR,
1333                         port->host_deviceid);
1334                 rio_mport_read_config_32(port, destid,
1335                         hopcount,
1336                         RIO_HOST_DID_LOCK_CSR, &result);
1337         }
1338
1339         return 0;
1340 }
1341 EXPORT_SYMBOL_GPL(rio_lock_device);
1342
1343 /**
1344  * rio_unlock_device - Releases host device lock for specified device
1345  * @port: Master port to send transaction
1346  * @destid: Destination ID for device/switch
1347  * @hopcount: Hopcount to reach switch
1348  *
1349  * Returns 0 if device lock released or EINVAL if fails.
1350  */
1351 int rio_unlock_device(struct rio_mport *port, u16 destid, u8 hopcount)
1352 {
1353         u32 result;
1354
1355         /* Release device lock */
1356         rio_mport_write_config_32(port, destid,
1357                                   hopcount,
1358                                   RIO_HOST_DID_LOCK_CSR,
1359                                   port->host_deviceid);
1360         rio_mport_read_config_32(port, destid, hopcount,
1361                 RIO_HOST_DID_LOCK_CSR, &result);
1362         if ((result & 0xffff) != 0xffff) {
1363                 pr_debug("RIO: badness when releasing device lock %x:%x\n",
1364                          destid, hopcount);
1365                 return -EINVAL;
1366         }
1367
1368         return 0;
1369 }
1370 EXPORT_SYMBOL_GPL(rio_unlock_device);
1371
1372 /**
1373  * rio_route_add_entry- Add a route entry to a switch routing table
1374  * @rdev: RIO device
1375  * @table: Routing table ID
1376  * @route_destid: Destination ID to be routed
1377  * @route_port: Port number to be routed
1378  * @lock: apply a hardware lock on switch device flag (1=lock, 0=no_lock)
1379  *
1380  * If available calls the switch specific add_entry() method to add a route
1381  * entry into a switch routing table. Otherwise uses standard RT update method
1382  * as defined by RapidIO specification. A specific routing table can be selected
1383  * using the @table argument if a switch has per port routing tables or
1384  * the standard (or global) table may be used by passing
1385  * %RIO_GLOBAL_TABLE in @table.
1386  *
1387  * Returns %0 on success or %-EINVAL on failure.
1388  */
1389 int rio_route_add_entry(struct rio_dev *rdev,
1390                         u16 table, u16 route_destid, u8 route_port, int lock)
1391 {
1392         int rc = -EINVAL;
1393         struct rio_switch_ops *ops = rdev->rswitch->ops;
1394
1395         if (lock) {
1396                 rc = rio_lock_device(rdev->net->hport, rdev->destid,
1397                                      rdev->hopcount, 1000);
1398                 if (rc)
1399                         return rc;
1400         }
1401
1402         spin_lock(&rdev->rswitch->lock);
1403
1404         if (ops == NULL || ops->add_entry == NULL) {
1405                 rc = rio_std_route_add_entry(rdev->net->hport, rdev->destid,
1406                                              rdev->hopcount, table,
1407                                              route_destid, route_port);
1408         } else if (try_module_get(ops->owner)) {
1409                 rc = ops->add_entry(rdev->net->hport, rdev->destid,
1410                                     rdev->hopcount, table, route_destid,
1411                                     route_port);
1412                 module_put(ops->owner);
1413         }
1414
1415         spin_unlock(&rdev->rswitch->lock);
1416
1417         if (lock)
1418                 rio_unlock_device(rdev->net->hport, rdev->destid,
1419                                   rdev->hopcount);
1420
1421         return rc;
1422 }
1423 EXPORT_SYMBOL_GPL(rio_route_add_entry);
1424
1425 /**
1426  * rio_route_get_entry- Read an entry from a switch routing table
1427  * @rdev: RIO device
1428  * @table: Routing table ID
1429  * @route_destid: Destination ID to be routed
1430  * @route_port: Pointer to read port number into
1431  * @lock: apply a hardware lock on switch device flag (1=lock, 0=no_lock)
1432  *
1433  * If available calls the switch specific get_entry() method to fetch a route
1434  * entry from a switch routing table. Otherwise uses standard RT read method
1435  * as defined by RapidIO specification. A specific routing table can be selected
1436  * using the @table argument if a switch has per port routing tables or
1437  * the standard (or global) table may be used by passing
1438  * %RIO_GLOBAL_TABLE in @table.
1439  *
1440  * Returns %0 on success or %-EINVAL on failure.
1441  */
1442 int rio_route_get_entry(struct rio_dev *rdev, u16 table,
1443                         u16 route_destid, u8 *route_port, int lock)
1444 {
1445         int rc = -EINVAL;
1446         struct rio_switch_ops *ops = rdev->rswitch->ops;
1447
1448         if (lock) {
1449                 rc = rio_lock_device(rdev->net->hport, rdev->destid,
1450                                      rdev->hopcount, 1000);
1451                 if (rc)
1452                         return rc;
1453         }
1454
1455         spin_lock(&rdev->rswitch->lock);
1456
1457         if (ops == NULL || ops->get_entry == NULL) {
1458                 rc = rio_std_route_get_entry(rdev->net->hport, rdev->destid,
1459                                              rdev->hopcount, table,
1460                                              route_destid, route_port);
1461         } else if (try_module_get(ops->owner)) {
1462                 rc = ops->get_entry(rdev->net->hport, rdev->destid,
1463                                     rdev->hopcount, table, route_destid,
1464                                     route_port);
1465                 module_put(ops->owner);
1466         }
1467
1468         spin_unlock(&rdev->rswitch->lock);
1469
1470         if (lock)
1471                 rio_unlock_device(rdev->net->hport, rdev->destid,
1472                                   rdev->hopcount);
1473         return rc;
1474 }
1475 EXPORT_SYMBOL_GPL(rio_route_get_entry);
1476
1477 /**
1478  * rio_route_clr_table - Clear a switch routing table
1479  * @rdev: RIO device
1480  * @table: Routing table ID
1481  * @lock: apply a hardware lock on switch device flag (1=lock, 0=no_lock)
1482  *
1483  * If available calls the switch specific clr_table() method to clear a switch
1484  * routing table. Otherwise uses standard RT write method as defined by RapidIO
1485  * specification. A specific routing table can be selected using the @table
1486  * argument if a switch has per port routing tables or the standard (or global)
1487  * table may be used by passing %RIO_GLOBAL_TABLE in @table.
1488  *
1489  * Returns %0 on success or %-EINVAL on failure.
1490  */
1491 int rio_route_clr_table(struct rio_dev *rdev, u16 table, int lock)
1492 {
1493         int rc = -EINVAL;
1494         struct rio_switch_ops *ops = rdev->rswitch->ops;
1495
1496         if (lock) {
1497                 rc = rio_lock_device(rdev->net->hport, rdev->destid,
1498                                      rdev->hopcount, 1000);
1499                 if (rc)
1500                         return rc;
1501         }
1502
1503         spin_lock(&rdev->rswitch->lock);
1504
1505         if (ops == NULL || ops->clr_table == NULL) {
1506                 rc = rio_std_route_clr_table(rdev->net->hport, rdev->destid,
1507                                              rdev->hopcount, table);
1508         } else if (try_module_get(ops->owner)) {
1509                 rc = ops->clr_table(rdev->net->hport, rdev->destid,
1510                                     rdev->hopcount, table);
1511
1512                 module_put(ops->owner);
1513         }
1514
1515         spin_unlock(&rdev->rswitch->lock);
1516
1517         if (lock)
1518                 rio_unlock_device(rdev->net->hport, rdev->destid,
1519                                   rdev->hopcount);
1520
1521         return rc;
1522 }
1523 EXPORT_SYMBOL_GPL(rio_route_clr_table);
1524
1525 #ifdef CONFIG_RAPIDIO_DMA_ENGINE
1526
1527 static bool rio_chan_filter(struct dma_chan *chan, void *arg)
1528 {
1529         struct rio_mport *mport = arg;
1530
1531         /* Check that DMA device belongs to the right MPORT */
1532         return mport == container_of(chan->device, struct rio_mport, dma);
1533 }
1534
1535 /**
1536  * rio_request_mport_dma - request RapidIO capable DMA channel associated
1537  *   with specified local RapidIO mport device.
1538  * @mport: RIO mport to perform DMA data transfers
1539  *
1540  * Returns pointer to allocated DMA channel or NULL if failed.
1541  */
1542 struct dma_chan *rio_request_mport_dma(struct rio_mport *mport)
1543 {
1544         dma_cap_mask_t mask;
1545
1546         dma_cap_zero(mask);
1547         dma_cap_set(DMA_SLAVE, mask);
1548         return dma_request_channel(mask, rio_chan_filter, mport);
1549 }
1550 EXPORT_SYMBOL_GPL(rio_request_mport_dma);
1551
1552 /**
1553  * rio_request_dma - request RapidIO capable DMA channel that supports
1554  *   specified target RapidIO device.
1555  * @rdev: RIO device associated with DMA transfer
1556  *
1557  * Returns pointer to allocated DMA channel or NULL if failed.
1558  */
1559 struct dma_chan *rio_request_dma(struct rio_dev *rdev)
1560 {
1561         return rio_request_mport_dma(rdev->net->hport);
1562 }
1563 EXPORT_SYMBOL_GPL(rio_request_dma);
1564
1565 /**
1566  * rio_release_dma - release specified DMA channel
1567  * @dchan: DMA channel to release
1568  */
1569 void rio_release_dma(struct dma_chan *dchan)
1570 {
1571         dma_release_channel(dchan);
1572 }
1573 EXPORT_SYMBOL_GPL(rio_release_dma);
1574
1575 /**
1576  * rio_dma_prep_xfer - RapidIO specific wrapper
1577  *   for device_prep_slave_sg callback defined by DMAENGINE.
1578  * @dchan: DMA channel to configure
1579  * @destid: target RapidIO device destination ID
1580  * @data: RIO specific data descriptor
1581  * @direction: DMA data transfer direction (TO or FROM the device)
1582  * @flags: dmaengine defined flags
1583  *
1584  * Initializes RapidIO capable DMA channel for the specified data transfer.
1585  * Uses DMA channel private extension to pass information related to remote
1586  * target RIO device.
1587  * Returns pointer to DMA transaction descriptor or NULL if failed.
1588  */
1589 struct dma_async_tx_descriptor *rio_dma_prep_xfer(struct dma_chan *dchan,
1590         u16 destid, struct rio_dma_data *data,
1591         enum dma_transfer_direction direction, unsigned long flags)
1592 {
1593         struct rio_dma_ext rio_ext;
1594
1595         if (dchan->device->device_prep_slave_sg == NULL) {
1596                 pr_err("%s: prep_rio_sg == NULL\n", __func__);
1597                 return NULL;
1598         }
1599
1600         rio_ext.destid = destid;
1601         rio_ext.rio_addr_u = data->rio_addr_u;
1602         rio_ext.rio_addr = data->rio_addr;
1603         rio_ext.wr_type = data->wr_type;
1604
1605         return dmaengine_prep_rio_sg(dchan, data->sg, data->sg_len,
1606                                      direction, flags, &rio_ext);
1607 }
1608 EXPORT_SYMBOL_GPL(rio_dma_prep_xfer);
1609
1610 /**
1611  * rio_dma_prep_slave_sg - RapidIO specific wrapper
1612  *   for device_prep_slave_sg callback defined by DMAENGINE.
1613  * @rdev: RIO device control structure
1614  * @dchan: DMA channel to configure
1615  * @data: RIO specific data descriptor
1616  * @direction: DMA data transfer direction (TO or FROM the device)
1617  * @flags: dmaengine defined flags
1618  *
1619  * Initializes RapidIO capable DMA channel for the specified data transfer.
1620  * Uses DMA channel private extension to pass information related to remote
1621  * target RIO device.
1622  * Returns pointer to DMA transaction descriptor or NULL if failed.
1623  */
1624 struct dma_async_tx_descriptor *rio_dma_prep_slave_sg(struct rio_dev *rdev,
1625         struct dma_chan *dchan, struct rio_dma_data *data,
1626         enum dma_transfer_direction direction, unsigned long flags)
1627 {
1628         return rio_dma_prep_xfer(dchan, rdev->destid, data, direction, flags);
1629 }
1630 EXPORT_SYMBOL_GPL(rio_dma_prep_slave_sg);
1631
1632 #endif /* CONFIG_RAPIDIO_DMA_ENGINE */
1633
1634 /**
1635  * rio_find_mport - find RIO mport by its ID
1636  * @mport_id: number (ID) of mport device
1637  *
1638  * Given a RIO mport number, the desired mport is located
1639  * in the global list of mports. If the mport is found, a pointer to its
1640  * data structure is returned.  If no mport is found, %NULL is returned.
1641  */
1642 struct rio_mport *rio_find_mport(int mport_id)
1643 {
1644         struct rio_mport *port;
1645
1646         mutex_lock(&rio_mport_list_lock);
1647         list_for_each_entry(port, &rio_mports, node) {
1648                 if (port->id == mport_id)
1649                         goto found;
1650         }
1651         port = NULL;
1652 found:
1653         mutex_unlock(&rio_mport_list_lock);
1654
1655         return port;
1656 }
1657
1658 /**
1659  * rio_register_scan - enumeration/discovery method registration interface
1660  * @mport_id: mport device ID for which fabric scan routine has to be set
1661  *            (RIO_MPORT_ANY = set for all available mports)
1662  * @scan_ops: enumeration/discovery operations structure
1663  *
1664  * Registers enumeration/discovery operations with RapidIO subsystem and
1665  * attaches it to the specified mport device (or all available mports
1666  * if RIO_MPORT_ANY is specified).
1667  *
1668  * Returns error if the mport already has an enumerator attached to it.
1669  * In case of RIO_MPORT_ANY skips mports with valid scan routines (no error).
1670  */
1671 int rio_register_scan(int mport_id, struct rio_scan *scan_ops)
1672 {
1673         struct rio_mport *port;
1674         struct rio_scan_node *scan;
1675         int rc = 0;
1676
1677         pr_debug("RIO: %s for mport_id=%d\n", __func__, mport_id);
1678
1679         if ((mport_id != RIO_MPORT_ANY && mport_id >= RIO_MAX_MPORTS) ||
1680             !scan_ops)
1681                 return -EINVAL;
1682
1683         mutex_lock(&rio_mport_list_lock);
1684
1685         /*
1686          * Check if there is another enumerator already registered for
1687          * the same mport ID (including RIO_MPORT_ANY). Multiple enumerators
1688          * for the same mport ID are not supported.
1689          */
1690         list_for_each_entry(scan, &rio_scans, node) {
1691                 if (scan->mport_id == mport_id) {
1692                         rc = -EBUSY;
1693                         goto err_out;
1694                 }
1695         }
1696
1697         /*
1698          * Allocate and initialize new scan registration node.
1699          */
1700         scan = kzalloc(sizeof(*scan), GFP_KERNEL);
1701         if (!scan) {
1702                 rc = -ENOMEM;
1703                 goto err_out;
1704         }
1705
1706         scan->mport_id = mport_id;
1707         scan->ops = scan_ops;
1708
1709         /*
1710          * Traverse the list of registered mports to attach this new scan.
1711          *
1712          * The new scan with matching mport ID overrides any previously attached
1713          * scan assuming that old scan (if any) is the default one (based on the
1714          * enumerator registration check above).
1715          * If the new scan is the global one, it will be attached only to mports
1716          * that do not have their own individual operations already attached.
1717          */
1718         list_for_each_entry(port, &rio_mports, node) {
1719                 if (port->id == mport_id) {
1720                         port->nscan = scan_ops;
1721                         break;
1722                 } else if (mport_id == RIO_MPORT_ANY && !port->nscan)
1723                         port->nscan = scan_ops;
1724         }
1725
1726         list_add_tail(&scan->node, &rio_scans);
1727
1728 err_out:
1729         mutex_unlock(&rio_mport_list_lock);
1730
1731         return rc;
1732 }
1733 EXPORT_SYMBOL_GPL(rio_register_scan);
1734
1735 /**
1736  * rio_unregister_scan - removes enumeration/discovery method from mport
1737  * @mport_id: mport device ID for which fabric scan routine has to be
1738  *            unregistered (RIO_MPORT_ANY = apply to all mports that use
1739  *            the specified scan_ops)
1740  * @scan_ops: enumeration/discovery operations structure
1741  *
1742  * Removes enumeration or discovery method assigned to the specified mport
1743  * device. If RIO_MPORT_ANY is specified, removes the specified operations from
1744  * all mports that have them attached.
1745  */
1746 int rio_unregister_scan(int mport_id, struct rio_scan *scan_ops)
1747 {
1748         struct rio_mport *port;
1749         struct rio_scan_node *scan;
1750
1751         pr_debug("RIO: %s for mport_id=%d\n", __func__, mport_id);
1752
1753         if (mport_id != RIO_MPORT_ANY && mport_id >= RIO_MAX_MPORTS)
1754                 return -EINVAL;
1755
1756         mutex_lock(&rio_mport_list_lock);
1757
1758         list_for_each_entry(port, &rio_mports, node)
1759                 if (port->id == mport_id ||
1760                     (mport_id == RIO_MPORT_ANY && port->nscan == scan_ops))
1761                         port->nscan = NULL;
1762
1763         list_for_each_entry(scan, &rio_scans, node) {
1764                 if (scan->mport_id == mport_id) {
1765                         list_del(&scan->node);
1766                         kfree(scan);
1767                         break;
1768                 }
1769         }
1770
1771         mutex_unlock(&rio_mport_list_lock);
1772
1773         return 0;
1774 }
1775 EXPORT_SYMBOL_GPL(rio_unregister_scan);
1776
1777 /**
1778  * rio_mport_scan - execute enumeration/discovery on the specified mport
1779  * @mport_id: number (ID) of mport device
1780  */
1781 int rio_mport_scan(int mport_id)
1782 {
1783         struct rio_mport *port = NULL;
1784         int rc;
1785
1786         mutex_lock(&rio_mport_list_lock);
1787         list_for_each_entry(port, &rio_mports, node) {
1788                 if (port->id == mport_id)
1789                         goto found;
1790         }
1791         mutex_unlock(&rio_mport_list_lock);
1792         return -ENODEV;
1793 found:
1794         if (!port->nscan) {
1795                 mutex_unlock(&rio_mport_list_lock);
1796                 return -EINVAL;
1797         }
1798
1799         if (!try_module_get(port->nscan->owner)) {
1800                 mutex_unlock(&rio_mport_list_lock);
1801                 return -ENODEV;
1802         }
1803
1804         mutex_unlock(&rio_mport_list_lock);
1805
1806         if (port->host_deviceid >= 0)
1807                 rc = port->nscan->enumerate(port, 0);
1808         else
1809                 rc = port->nscan->discover(port, RIO_SCAN_ENUM_NO_WAIT);
1810
1811         module_put(port->nscan->owner);
1812         return rc;
1813 }
1814
1815 static void rio_fixup_device(struct rio_dev *dev)
1816 {
1817 }
1818
1819 static int rio_init(void)
1820 {
1821         struct rio_dev *dev = NULL;
1822
1823         while ((dev = rio_get_device(RIO_ANY_ID, RIO_ANY_ID, dev)) != NULL) {
1824                 rio_fixup_device(dev);
1825         }
1826         return 0;
1827 }
1828
1829 static struct workqueue_struct *rio_wq;
1830
1831 struct rio_disc_work {
1832         struct work_struct      work;
1833         struct rio_mport        *mport;
1834 };
1835
1836 static void disc_work_handler(struct work_struct *_work)
1837 {
1838         struct rio_disc_work *work;
1839
1840         work = container_of(_work, struct rio_disc_work, work);
1841         pr_debug("RIO: discovery work for mport %d %s\n",
1842                  work->mport->id, work->mport->name);
1843         if (try_module_get(work->mport->nscan->owner)) {
1844                 work->mport->nscan->discover(work->mport, 0);
1845                 module_put(work->mport->nscan->owner);
1846         }
1847 }
1848
1849 int rio_init_mports(void)
1850 {
1851         struct rio_mport *port;
1852         struct rio_disc_work *work;
1853         int n = 0;
1854
1855         if (!next_portid)
1856                 return -ENODEV;
1857
1858         /*
1859          * First, run enumerations and check if we need to perform discovery
1860          * on any of the registered mports.
1861          */
1862         mutex_lock(&rio_mport_list_lock);
1863         list_for_each_entry(port, &rio_mports, node) {
1864                 if (port->host_deviceid >= 0) {
1865                         if (port->nscan && try_module_get(port->nscan->owner)) {
1866                                 port->nscan->enumerate(port, 0);
1867                                 module_put(port->nscan->owner);
1868                         }
1869                 } else
1870                         n++;
1871         }
1872         mutex_unlock(&rio_mport_list_lock);
1873
1874         if (!n)
1875                 goto no_disc;
1876
1877         /*
1878          * If we have mports that require discovery schedule a discovery work
1879          * for each of them. If the code below fails to allocate needed
1880          * resources, exit without error to keep results of enumeration
1881          * process (if any).
1882          * TODO: Implement restart of discovery process for all or
1883          * individual discovering mports.
1884          */
1885         rio_wq = alloc_workqueue("riodisc", 0, 0);
1886         if (!rio_wq) {
1887                 pr_err("RIO: unable allocate rio_wq\n");
1888                 goto no_disc;
1889         }
1890
1891         work = kcalloc(n, sizeof *work, GFP_KERNEL);
1892         if (!work) {
1893                 pr_err("RIO: no memory for work struct\n");
1894                 destroy_workqueue(rio_wq);
1895                 goto no_disc;
1896         }
1897
1898         n = 0;
1899         mutex_lock(&rio_mport_list_lock);
1900         list_for_each_entry(port, &rio_mports, node) {
1901                 if (port->host_deviceid < 0 && port->nscan) {
1902                         work[n].mport = port;
1903                         INIT_WORK(&work[n].work, disc_work_handler);
1904                         queue_work(rio_wq, &work[n].work);
1905                         n++;
1906                 }
1907         }
1908
1909         flush_workqueue(rio_wq);
1910         mutex_unlock(&rio_mport_list_lock);
1911         pr_debug("RIO: destroy discovery workqueue\n");
1912         destroy_workqueue(rio_wq);
1913         kfree(work);
1914
1915 no_disc:
1916         rio_init();
1917
1918         return 0;
1919 }
1920
1921 static int rio_get_hdid(int index)
1922 {
1923         if (ids_num == 0 || ids_num <= index || index >= RIO_MAX_MPORTS)
1924                 return -1;
1925
1926         return hdid[index];
1927 }
1928
1929 int rio_register_mport(struct rio_mport *port)
1930 {
1931         struct rio_scan_node *scan = NULL;
1932         int res = 0;
1933
1934         if (next_portid >= RIO_MAX_MPORTS) {
1935                 pr_err("RIO: reached specified max number of mports\n");
1936                 return 1;
1937         }
1938
1939         port->id = next_portid++;
1940         port->host_deviceid = rio_get_hdid(port->id);
1941         port->nscan = NULL;
1942
1943         dev_set_name(&port->dev, "rapidio%d", port->id);
1944         port->dev.class = &rio_mport_class;
1945
1946         res = device_register(&port->dev);
1947         if (res)
1948                 dev_err(&port->dev, "RIO: mport%d registration failed ERR=%d\n",
1949                         port->id, res);
1950         else
1951                 dev_dbg(&port->dev, "RIO: mport%d registered\n", port->id);
1952
1953         mutex_lock(&rio_mport_list_lock);
1954         list_add_tail(&port->node, &rio_mports);
1955
1956         /*
1957          * Check if there are any registered enumeration/discovery operations
1958          * that have to be attached to the added mport.
1959          */
1960         list_for_each_entry(scan, &rio_scans, node) {
1961                 if (port->id == scan->mport_id ||
1962                     scan->mport_id == RIO_MPORT_ANY) {
1963                         port->nscan = scan->ops;
1964                         if (port->id == scan->mport_id)
1965                                 break;
1966                 }
1967         }
1968         mutex_unlock(&rio_mport_list_lock);
1969
1970         pr_debug("RIO: %s %s id=%d\n", __func__, port->name, port->id);
1971         return 0;
1972 }
1973 EXPORT_SYMBOL_GPL(rio_register_mport);
1974
1975 EXPORT_SYMBOL_GPL(rio_local_get_device_id);
1976 EXPORT_SYMBOL_GPL(rio_get_device);
1977 EXPORT_SYMBOL_GPL(rio_get_asm);
1978 EXPORT_SYMBOL_GPL(rio_request_inb_dbell);
1979 EXPORT_SYMBOL_GPL(rio_release_inb_dbell);
1980 EXPORT_SYMBOL_GPL(rio_request_outb_dbell);
1981 EXPORT_SYMBOL_GPL(rio_release_outb_dbell);
1982 EXPORT_SYMBOL_GPL(rio_request_inb_mbox);
1983 EXPORT_SYMBOL_GPL(rio_release_inb_mbox);
1984 EXPORT_SYMBOL_GPL(rio_request_outb_mbox);
1985 EXPORT_SYMBOL_GPL(rio_release_outb_mbox);
1986 EXPORT_SYMBOL_GPL(rio_init_mports);