4 files changed, 48 insertions, 2 deletions

diff --git a/drivers/firewire/core-transaction.c b/drivers/firewire/core-transaction.c
index dd8ef650a7cb..e0c6cce894cf 100644
--- a/drivers/firewire/core-transaction.c
+++ b/drivers/firewire/core-transaction.c
@@ -1003,7 +1003,12 @@ static u32 read_state_register(struct fw_card *card)
          *      reset, but then cleared when the units are ready again, which
          *      happens immediately for us.
          */
-        return 0;
+        u32 value = 0x0000;
+
+        /* Bit 8 (cmstr): */
+        value |= card->driver->read_csr_reg(card, CSR_STATE_CLEAR);
+
+        return value;
 }
 
 static void update_split_timeout(struct fw_card *card)
@@ -1034,6 +1039,8 @@ static void handle_registers(struct fw_card *card, struct fw_request *request,
                 if (tcode == TCODE_READ_QUADLET_REQUEST) {
                         *data = cpu_to_be32(read_state_register(card));
                 } else if (tcode == TCODE_WRITE_QUADLET_REQUEST) {
+                        card->driver->write_csr_reg(card, CSR_STATE_CLEAR,
+                                                    be32_to_cpu(*data));
                 } else {
                         rcode = RCODE_TYPE_ERROR;
                 }
@@ -1043,7 +1050,8 @@ static void handle_registers(struct fw_card *card, struct fw_request *request,
                 if (tcode == TCODE_READ_QUADLET_REQUEST) {
                         *data = cpu_to_be32(read_state_register(card));
                 } else if (tcode == TCODE_WRITE_QUADLET_REQUEST) {
-                        /* FIXME: implement cmstr */
+                        card->driver->write_csr_reg(card, CSR_STATE_SET,
+                                                    be32_to_cpu(*data));
                         /* FIXME: implement abdicate */
                 } else {
                         rcode = RCODE_TYPE_ERROR;
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index 3b8c0f042f49..aaecdd1c1767 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -40,6 +40,8 @@ struct fw_packet;
 
 #define FEATURE_PRIORITY_BUDGET         0x01
 
+#define CSR_STATE_BIT_CMSTR     (1 << 8)
+
 struct fw_card_driver {
         /*
          * Enable the given card with the given initial config rom.
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index 0e5413531785..1dcc2e427eb1 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -172,6 +172,7 @@ struct fw_ohci {
         unsigned quirks;
         unsigned int pri_req_max;
         u32 bus_time;
+        bool is_root;
 
         /*
          * Spinlock for accessing fw_ohci data.  Never call out of
@@ -1400,6 +1401,7 @@ static void bus_reset_tasklet(unsigned long data)
         unsigned long flags;
         void *free_rom = NULL;
         dma_addr_t free_rom_bus = 0;
+        bool is_new_root;
 
         reg = reg_read(ohci, OHCI1394_NodeID);
         if (!(reg & OHCI1394_NodeID_idValid)) {
@@ -1413,6 +1415,12 @@ static void bus_reset_tasklet(unsigned long data)
         ohci->node_id = reg & (OHCI1394_NodeID_busNumber |
                                OHCI1394_NodeID_nodeNumber);
 
+        is_new_root = (reg & OHCI1394_NodeID_root) != 0;
+        if (!(ohci->is_root && is_new_root))
+                reg_write(ohci, OHCI1394_LinkControlSet,
+                          OHCI1394_LinkControl_cycleMaster);
+        ohci->is_root = is_new_root;
+
         reg = reg_read(ohci, OHCI1394_SelfIDCount);
         if (reg & OHCI1394_SelfIDCount_selfIDError) {
                 fw_notify("inconsistent self IDs\n");
@@ -2013,6 +2021,16 @@ static u32 ohci_read_csr_reg(struct fw_card *card, int csr_offset)
         u32 value;
 
         switch (csr_offset) {
+        case CSR_STATE_CLEAR:
+        case CSR_STATE_SET:
+                /* the controller driver handles only the cmstr bit */
+                if (ohci->is_root &&
+                    (reg_read(ohci, OHCI1394_LinkControlSet) &
+                     OHCI1394_LinkControl_cycleMaster))
+                        return CSR_STATE_BIT_CMSTR;
+                else
+                        return 0;
+
         case CSR_NODE_IDS:
                 return reg_read(ohci, OHCI1394_NodeID) << 16;
 
@@ -2050,6 +2068,23 @@ static void ohci_write_csr_reg(struct fw_card *card, int csr_offset, u32 value)
         unsigned long flags;
 
         switch (csr_offset) {
+        case CSR_STATE_CLEAR:
+                /* the controller driver handles only the cmstr bit */
+                if ((value & CSR_STATE_BIT_CMSTR) && ohci->is_root) {
+                        reg_write(ohci, OHCI1394_LinkControlClear,
+                                  OHCI1394_LinkControl_cycleMaster);
+                        flush_writes(ohci);
+                }
+                break;
+
+        case CSR_STATE_SET:
+                if ((value & CSR_STATE_BIT_CMSTR) && ohci->is_root) {
+                        reg_write(ohci, OHCI1394_LinkControlSet,
+                                  OHCI1394_LinkControl_cycleMaster);
+                        flush_writes(ohci);
+                }
+                break;
+
         case CSR_NODE_IDS:
                 reg_write(ohci, OHCI1394_NodeID, value >> 16);
                 flush_writes(ohci);
diff --git a/drivers/firewire/ohci.h b/drivers/firewire/ohci.h
index 3bc9a5d744eb..0e6c5a466908 100644
--- a/drivers/firewire/ohci.h
+++ b/drivers/firewire/ohci.h
@@ -60,6 +60,7 @@
 #define   OHCI1394_LinkControl_cycleSource      (1 << 22)
 #define OHCI1394_NodeID                       0x0E8
 #define   OHCI1394_NodeID_idValid             0x80000000
+#define   OHCI1394_NodeID_root                0x40000000
 #define   OHCI1394_NodeID_nodeNumber          0x0000003f
 #define   OHCI1394_NodeID_busNumber           0x0000ffc0
 #define OHCI1394_PhyControl                   0x0EC