1 files changed, 124 insertions, 64 deletions

diff --git a/drivers/firewire/fw-ohci.c b/drivers/firewire/fw-ohci.c
index c17342d3e6fd..0d08bf9b78c2 100644
--- a/drivers/firewire/fw-ohci.c
+++ b/drivers/firewire/fw-ohci.c
@@ -268,7 +268,7 @@ static int ar_context_add_page(struct ar_context *ctx)
 
         dma_sync_single_for_device(dev, ab_bus, PAGE_SIZE, DMA_BIDIRECTIONAL);
 
-        ctx->last_buffer->descriptor.branch_address = ab_bus | 1;
+        ctx->last_buffer->descriptor.branch_address = cpu_to_le32(ab_bus | 1);
         ctx->last_buffer->next = ab;
         ctx->last_buffer = ab;
 
@@ -417,11 +417,21 @@ ar_context_init(struct ar_context *ctx, struct fw_ohci *ohci, u32 regs)
         ctx->current_buffer = ab.next;
         ctx->pointer = ctx->current_buffer->data;
 
-        reg_write(ctx->ohci, COMMAND_PTR(ctx->regs), ab.descriptor.branch_address);
+        return 0;
+}
+
+static void ar_context_run(struct ar_context *ctx)
+{
+        struct ar_buffer *ab = ctx->current_buffer;
+        dma_addr_t ab_bus;
+        size_t offset;
+
+        offset = offsetof(struct ar_buffer, data);
+        ab_bus = ab->descriptor.data_address - offset;
+
+        reg_write(ctx->ohci, COMMAND_PTR(ctx->regs), ab_bus | 1);
         reg_write(ctx->ohci, CONTROL_SET(ctx->regs), CONTEXT_RUN);
         flush_writes(ctx->ohci);
-
-        return 0;
 }
 
 static void context_tasklet(unsigned long data)
@@ -1038,11 +1048,78 @@ static irqreturn_t irq_handler(int irq, void *data)
         return IRQ_HANDLED;
 }
 
+static int software_reset(struct fw_ohci *ohci)
+{
+        int i;
+
+        reg_write(ohci, OHCI1394_HCControlSet, OHCI1394_HCControl_softReset);
+
+        for (i = 0; i < OHCI_LOOP_COUNT; i++) {
+                if ((reg_read(ohci, OHCI1394_HCControlSet) &
+                     OHCI1394_HCControl_softReset) == 0)
+                        return 0;
+                msleep(1);
+        }
+
+        return -EBUSY;
+}
+
 static int ohci_enable(struct fw_card *card, u32 *config_rom, size_t length)
 {
         struct fw_ohci *ohci = fw_ohci(card);
         struct pci_dev *dev = to_pci_dev(card->device);
 
+        if (software_reset(ohci)) {
+                fw_error("Failed to reset ohci card.\n");
+                return -EBUSY;
+        }
+
+        /*
+         * Now enable LPS, which we need in order to start accessing
+         * most of the registers.  In fact, on some cards (ALI M5251),
+         * accessing registers in the SClk domain without LPS enabled
+         * will lock up the machine.  Wait 50msec to make sure we have
+         * full link enabled.
+         */
+        reg_write(ohci, OHCI1394_HCControlSet,
+                  OHCI1394_HCControl_LPS |
+                  OHCI1394_HCControl_postedWriteEnable);
+        flush_writes(ohci);
+        msleep(50);
+
+        reg_write(ohci, OHCI1394_HCControlClear,
+                  OHCI1394_HCControl_noByteSwapData);
+
+        reg_write(ohci, OHCI1394_LinkControlSet,
+                  OHCI1394_LinkControl_rcvSelfID |
+                  OHCI1394_LinkControl_cycleTimerEnable |
+                  OHCI1394_LinkControl_cycleMaster);
+
+        reg_write(ohci, OHCI1394_ATRetries,
+                  OHCI1394_MAX_AT_REQ_RETRIES |
+                  (OHCI1394_MAX_AT_RESP_RETRIES << 4) |
+                  (OHCI1394_MAX_PHYS_RESP_RETRIES << 8));
+
+        ar_context_run(&ohci->ar_request_ctx);
+        ar_context_run(&ohci->ar_response_ctx);
+
+        reg_write(ohci, OHCI1394_SelfIDBuffer, ohci->self_id_bus);
+        reg_write(ohci, OHCI1394_PhyUpperBound, 0x00010000);
+        reg_write(ohci, OHCI1394_IntEventClear, ~0);
+        reg_write(ohci, OHCI1394_IntMaskClear, ~0);
+        reg_write(ohci, OHCI1394_IntMaskSet,
+                  OHCI1394_selfIDComplete |
+                  OHCI1394_RQPkt | OHCI1394_RSPkt |
+                  OHCI1394_reqTxComplete | OHCI1394_respTxComplete |
+                  OHCI1394_isochRx | OHCI1394_isochTx |
+                  OHCI1394_masterIntEnable |
+                  OHCI1394_cycle64Seconds);
+
+        /* Activate link_on bit and contender bit in our self ID packets.*/
+        if (ohci_update_phy_reg(card, 4, 0,
+                                PHY_LINK_ACTIVE | PHY_CONTENDER) < 0)
+                return -EIO;
+
         /*
          * When the link is not yet enabled, the atomic config rom
          * update mechanism described below in ohci_set_config_rom()
@@ -1700,22 +1777,6 @@ static const struct fw_card_driver ohci_driver = {
         .stop_iso               = ohci_stop_iso,
 };
 
-static int software_reset(struct fw_ohci *ohci)
-{
-        int i;
-
-        reg_write(ohci, OHCI1394_HCControlSet, OHCI1394_HCControl_softReset);
-
-        for (i = 0; i < OHCI_LOOP_COUNT; i++) {
-                if ((reg_read(ohci, OHCI1394_HCControlSet) &
-                     OHCI1394_HCControl_softReset) == 0)
-                        return 0;
-                msleep(1);
-        }
-
-        return -EBUSY;
-}
-
 static int __devinit
 pci_probe(struct pci_dev *dev, const struct pci_device_id *ent)
 {
@@ -1761,33 +1822,6 @@ pci_probe(struct pci_dev *dev, const struct pci_device_id *ent)
                 goto fail_iomem;
         }
 
-        if (software_reset(ohci)) {
-                fw_error("Failed to reset ohci card.\n");
-                err = -EBUSY;
-                goto fail_registers;
-        }
-
-        /*
-         * Now enable LPS, which we need in order to start accessing
-         * most of the registers.  In fact, on some cards (ALI M5251),
-         * accessing registers in the SClk domain without LPS enabled
-         * will lock up the machine.  Wait 50msec to make sure we have
-         * full link enabled.
-         */
-        reg_write(ohci, OHCI1394_HCControlSet,
-                  OHCI1394_HCControl_LPS |
-                  OHCI1394_HCControl_postedWriteEnable);
-        flush_writes(ohci);
-        msleep(50);
-
-        reg_write(ohci, OHCI1394_HCControlClear,
-                  OHCI1394_HCControl_noByteSwapData);
-
-        reg_write(ohci, OHCI1394_LinkControlSet,
-                  OHCI1394_LinkControl_rcvSelfID |
-                  OHCI1394_LinkControl_cycleTimerEnable |
-                  OHCI1394_LinkControl_cycleMaster);
-
         ar_context_init(&ohci->ar_request_ctx, ohci,
                         OHCI1394_AsReqRcvContextControlSet);
 
@@ -1800,11 +1834,6 @@ pci_probe(struct pci_dev *dev, const struct pci_device_id *ent)
         context_init(&ohci->at_response_ctx, ohci, AT_BUFFER_SIZE,
                      OHCI1394_AsRspTrContextControlSet, handle_at_packet);
 
-        reg_write(ohci, OHCI1394_ATRetries,
-                  OHCI1394_MAX_AT_REQ_RETRIES |
-                  (OHCI1394_MAX_AT_RESP_RETRIES << 4) |
-                  (OHCI1394_MAX_PHYS_RESP_RETRIES << 8));
-
         reg_write(ohci, OHCI1394_IsoRecvIntMaskSet, ~0);
         ohci->it_context_mask = reg_read(ohci, OHCI1394_IsoRecvIntMaskSet);
         reg_write(ohci, OHCI1394_IsoRecvIntMaskClear, ~0);
@@ -1834,18 +1863,6 @@ pci_probe(struct pci_dev *dev, const struct pci_device_id *ent)
                 goto fail_registers;
         }
 
-        reg_write(ohci, OHCI1394_SelfIDBuffer, ohci->self_id_bus);
-        reg_write(ohci, OHCI1394_PhyUpperBound, 0x00010000);
-        reg_write(ohci, OHCI1394_IntEventClear, ~0);
-        reg_write(ohci, OHCI1394_IntMaskClear, ~0);
-        reg_write(ohci, OHCI1394_IntMaskSet,
-                  OHCI1394_selfIDComplete |
-                  OHCI1394_RQPkt | OHCI1394_RSPkt |
-                  OHCI1394_reqTxComplete | OHCI1394_respTxComplete |
-                  OHCI1394_isochRx | OHCI1394_isochTx |
-                  OHCI1394_masterIntEnable |
-                  OHCI1394_cycle64Seconds);
-
         bus_options = reg_read(ohci, OHCI1394_BusOptions);
         max_receive = (bus_options >> 12) & 0xf;
         link_speed = bus_options & 0x7;
@@ -1907,6 +1924,45 @@ static void pci_remove(struct pci_dev *dev)
         fw_notify("Removed fw-ohci device.\n");
 }
 
+#ifdef CONFIG_PM
+static int pci_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+        struct fw_ohci *ohci = pci_get_drvdata(pdev);
+        int err;
+
+        software_reset(ohci);
+        free_irq(pdev->irq, ohci);
+        err = pci_save_state(pdev);
+        if (err) {
+                fw_error("pci_save_state failed with %d", err);
+                return err;
+        }
+        err = pci_set_power_state(pdev, pci_choose_state(pdev, state));
+        if (err) {
+                fw_error("pci_set_power_state failed with %d", err);
+                return err;
+        }
+
+        return 0;
+}
+
+static int pci_resume(struct pci_dev *pdev)
+{
+        struct fw_ohci *ohci = pci_get_drvdata(pdev);
+        int err;
+
+        pci_set_power_state(pdev, PCI_D0);
+        pci_restore_state(pdev);
+        err = pci_enable_device(pdev);
+        if (err) {
+                fw_error("pci_enable_device failed with %d", err);
+                return err;
+        }
+
+        return ohci_enable(&ohci->card, ohci->config_rom, CONFIG_ROM_SIZE);
+}
+#endif
+
 static struct pci_device_id pci_table[] = {
         { PCI_DEVICE_CLASS(PCI_CLASS_SERIAL_FIREWIRE_OHCI, ~0) },
         { }
@@ -1919,6 +1975,10 @@ static struct pci_driver fw_ohci_pci_driver = {
         .id_table       = pci_table,
         .probe          = pci_probe,
         .remove         = pci_remove,
+#ifdef CONFIG_PM
+        .resume         = pci_resume,
+        .suspend        = pci_suspend,
+#endif
 };
 
 MODULE_AUTHOR("Kristian Hoegsberg <krh@bitplanet.net>");