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