3 files changed, 154 insertions, 21 deletions

diff --git a/drivers/firewire/fw-ohci.c b/drivers/firewire/fw-ohci.c
index 5156329a8655..ac6c018de0dc 100644
--- a/drivers/firewire/fw-ohci.c
+++ b/drivers/firewire/fw-ohci.c
@@ -540,38 +540,136 @@ at_context_init(struct at_context *ctx, struct fw_ohci *ohci, u32 control_set)
 }
 
 #define header_get_destination(q)       (((q) >> 16) & 0xffff)
+#define header_get_tcode(q)             (((q) >> 4) & 0x0f)
+#define header_get_offset_high(q)       (((q) >> 0) & 0xffff)
+#define header_get_data_length(q)       (((q) >> 16) & 0xffff)
+#define header_get_extended_tcode(q)    (((q) >> 0) & 0xffff)
+
+static void
+handle_local_rom(struct fw_ohci *ohci, struct fw_packet *packet, u32 csr)
+{
+        struct fw_packet response;
+        int tcode, length, i;
+
+        tcode = header_get_tcode(packet->header[0]);
+        if (TCODE_IS_BLOCK_PACKET(tcode))
+                length = header_get_data_length(packet->header[3]);
+        else
+                length = 4;
+
+        i = csr - CSR_CONFIG_ROM;
+        if (i + length > CONFIG_ROM_SIZE) {
+                fw_fill_response(&response, packet->header,
+                                 RCODE_ADDRESS_ERROR, NULL, 0);
+        } else if (!TCODE_IS_READ_REQUEST(tcode)) {
+                fw_fill_response(&response, packet->header,
+                                 RCODE_TYPE_ERROR, NULL, 0);
+        } else {
+                fw_fill_response(&response, packet->header, RCODE_COMPLETE,
+                                 (void *) ohci->config_rom + i, length);
+        }
+
+        fw_core_handle_response(&ohci->card, &response);
+}
+
+static void
+handle_local_lock(struct fw_ohci *ohci, struct fw_packet *packet, u32 csr)
+{
+        struct fw_packet response;
+        int tcode, length, ext_tcode, sel;
+        __be32 *payload, lock_old;
+        u32 lock_arg, lock_data;
+
+        tcode = header_get_tcode(packet->header[0]);
+        length = header_get_data_length(packet->header[3]);
+        payload = packet->payload;
+        ext_tcode = header_get_extended_tcode(packet->header[3]);
+
+        if (tcode == TCODE_LOCK_REQUEST &&
+            ext_tcode == EXTCODE_COMPARE_SWAP && length == 8) {
+                lock_arg = be32_to_cpu(payload[0]);
+                lock_data = be32_to_cpu(payload[1]);
+        } else if (tcode == TCODE_READ_QUADLET_REQUEST) {
+                lock_arg = 0;
+                lock_data = 0;
+        } else {
+                fw_fill_response(&response, packet->header,
+                                 RCODE_TYPE_ERROR, NULL, 0);
+                goto out;
+        }
+
+        sel = (csr - CSR_BUS_MANAGER_ID) / 4;
+        reg_write(ohci, OHCI1394_CSRData, lock_data);
+        reg_write(ohci, OHCI1394_CSRCompareData, lock_arg);
+        reg_write(ohci, OHCI1394_CSRControl, sel);
+
+        if (reg_read(ohci, OHCI1394_CSRControl) & 0x80000000)
+                lock_old = cpu_to_be32(reg_read(ohci, OHCI1394_CSRData));
+        else
+                fw_notify("swap not done yet\n");
+
+        fw_fill_response(&response, packet->header,
+                         RCODE_COMPLETE, &lock_old, sizeof lock_old);
+ out:
+        fw_core_handle_response(&ohci->card, &response);
+}
+
+static void
+handle_local_request(struct at_context *ctx, struct fw_packet *packet)
+{
+        u64 offset;
+        u32 csr;
+
+        packet->ack = ACK_PENDING;
+        packet->callback(packet, &ctx->ohci->card, packet->ack);
+
+        offset =
+                ((unsigned long long)
+                 header_get_offset_high(packet->header[1]) << 32) |
+                packet->header[2];
+        csr = offset - CSR_REGISTER_BASE;
+
+        /* Handle config rom reads. */
+        if (csr >= CSR_CONFIG_ROM && csr < CSR_CONFIG_ROM_END)
+                handle_local_rom(ctx->ohci, packet, csr);
+        else switch (csr) {
+        case CSR_BUS_MANAGER_ID:
+        case CSR_BANDWIDTH_AVAILABLE:
+        case CSR_CHANNELS_AVAILABLE_HI:
+        case CSR_CHANNELS_AVAILABLE_LO:
+                handle_local_lock(ctx->ohci, packet, csr);
+                break;
+        default:
+                if (ctx == &ctx->ohci->at_request_ctx)
+                        fw_core_handle_request(&ctx->ohci->card, packet);
+                else
+                        fw_core_handle_response(&ctx->ohci->card, packet);
+                break;
+        }
+}
 
 static void
 at_context_transmit(struct at_context *ctx, struct fw_packet *packet)
 {
         LIST_HEAD(list);
         unsigned long flags;
-        int local;
 
         spin_lock_irqsave(&ctx->ohci->lock, flags);
 
         if (header_get_destination(packet->header[0]) == ctx->ohci->node_id &&
             ctx->ohci->generation == packet->generation) {
-                local = 1;
-        } else {
-                list_add_tail(&packet->link, &ctx->list);
-                if (ctx->list.next == &packet->link)
-                        at_context_setup_packet(ctx, &list);
-                local = 0;
+                spin_unlock_irqrestore(&ctx->ohci->lock, flags);
+                handle_local_request(ctx, packet);
+                return;
         }
 
+        list_add_tail(&packet->link, &ctx->list);
+        if (ctx->list.next == &packet->link)
+                at_context_setup_packet(ctx, &list);
+
         spin_unlock_irqrestore(&ctx->ohci->lock, flags);
 
         do_packet_callbacks(ctx->ohci, &list);
-
-        if (local) {
-                packet->ack = ACK_PENDING;
-                packet->callback(packet, &ctx->ohci->card, packet->ack);
-                if (ctx == &ctx->ohci->at_request_ctx)
-                        fw_core_handle_request(&ctx->ohci->card, packet);
-                else
-                        fw_core_handle_response(&ctx->ohci->card, packet);
-        }
 }
 
 static void bus_reset_tasklet(unsigned long data)
diff --git a/drivers/firewire/fw-transaction.c b/drivers/firewire/fw-transaction.c
index a116ffa9bf45..780ed2b44983 100644
--- a/drivers/firewire/fw-transaction.c
+++ b/drivers/firewire/fw-transaction.c
@@ -425,7 +425,7 @@ free_response_callback(struct fw_packet *packet,
         kfree(request);
 }
 
-static void
+void
 fw_fill_response(struct fw_packet *response, u32 *request_header,
                  int rcode, void *payload, size_t length)
 {
@@ -457,7 +457,10 @@ fw_fill_response(struct fw_packet *response, u32 *request_header,
         case TCODE_READ_QUADLET_REQUEST:
                 response->header[0] |=
                         header_tcode(TCODE_READ_QUADLET_RESPONSE);
-                response->header[3] = *(u32 *)payload;
+                if (payload != NULL)
+                        response->header[3] = *(u32 *)payload;
+                else
+                        response->header[3] = 0;
                 response->header_length = 16;
                 response->payload_length = 0;
                 break;
@@ -478,6 +481,7 @@ fw_fill_response(struct fw_packet *response, u32 *request_header,
                 return;
         }
 }
+EXPORT_SYMBOL(fw_fill_response);
 
 static struct fw_request *
 allocate_request(struct fw_packet *p)
@@ -529,9 +533,9 @@ allocate_request(struct fw_packet *p)
         request->response.generation = p->generation;
         request->response.callback = free_response_callback;
         request->ack = p->ack;
-        request->length = p->payload_length;
+        request->length = length;
         if (data)
-                memcpy(request->data, p->payload, p->payload_length);
+                memcpy(request->data, p->payload, length);
 
         memcpy(request->request_header, p->header, sizeof p->header);
 
@@ -656,7 +660,7 @@ fw_core_handle_response(struct fw_card *card, struct fw_packet *p)
 
         case TCODE_READ_BLOCK_RESPONSE:
         case TCODE_LOCK_RESPONSE:
-                data = &p->header[4];
+                data = p->payload;
                 data_length = header_get_data_length(p->header[3]);
                 break;
 
diff --git a/drivers/firewire/fw-transaction.h b/drivers/firewire/fw-transaction.h
index 903235b142f2..fb46ef78a323 100644
--- a/drivers/firewire/fw-transaction.h
+++ b/drivers/firewire/fw-transaction.h
@@ -40,6 +40,7 @@
 #define TCODE_STREAM_DATA               10
 #define TCODE_LOCK_RESPONSE             11
 
+#define TCODE_IS_READ_REQUEST(tcode)    (((tcode) & ~1) == 4)
 #define TCODE_IS_BLOCK_PACKET(tcode)    (((tcode) &  1) != 0)
 #define TCODE_IS_REQUEST(tcode)         (((tcode) &  2) == 0)
 #define TCODE_IS_RESPONSE(tcode)        (((tcode) &  2) != 0)
@@ -103,6 +104,34 @@
 #define PHY_PACKET_LINK_ON      0x1
 #define PHY_PACKET_SELF_ID      0x2
 
+#define CSR_REGISTER_BASE               0xfffff0000000ULL
+
+/* register offsets relative to CSR_REGISTER_BASE */
+#define CSR_STATE_CLEAR                 0x0
+#define CSR_STATE_SET                   0x4
+#define CSR_NODE_IDS                    0x8
+#define CSR_RESET_START                 0xc
+#define CSR_SPLIT_TIMEOUT_HI            0x18
+#define CSR_SPLIT_TIMEOUT_LO            0x1c
+#define CSR_CYCLE_TIME                  0x200
+#define CSR_BUS_TIME                    0x204
+#define CSR_BUSY_TIMEOUT                0x210
+#define CSR_BUS_MANAGER_ID              0x21c
+#define CSR_BANDWIDTH_AVAILABLE         0x220
+#define CSR_CHANNELS_AVAILABLE          0x224
+#define CSR_CHANNELS_AVAILABLE_HI       0x224
+#define CSR_CHANNELS_AVAILABLE_LO       0x228
+#define CSR_BROADCAST_CHANNEL           0x234
+#define CSR_CONFIG_ROM                  0x400
+#define CSR_CONFIG_ROM_END              0x800
+#define CSR_FCP_COMMAND                 0xB00
+#define CSR_FCP_RESPONSE                0xD00
+#define CSR_FCP_END                     0xF00
+#define CSR_TOPOLOGY_MAP                0x1000
+#define CSR_TOPOLOGY_MAP_END            0x1400
+#define CSR_SPEED_MAP                   0x2000
+#define CSR_SPEED_MAP_END               0x3000
+
 #define fw_notify(s, args...) printk(KERN_NOTICE KBUILD_MODNAME ": " s, ## args)
 #define fw_error(s, args...) printk(KERN_ERR KBUILD_MODNAME ": " s, ## args)
 #define fw_debug(s, args...) printk(KERN_DEBUG KBUILD_MODNAME ": " s, ## args)
@@ -227,6 +256,8 @@ extern const struct fw_address_region fw_unit_space_region;
 int fw_core_add_address_handler(struct fw_address_handler *handler,
                                 const struct fw_address_region *region);
 void fw_core_remove_address_handler(struct fw_address_handler *handler);
+void fw_fill_response(struct fw_packet *response, u32 *request_header,
+                      int rcode, void *payload, size_t length);
 void fw_send_response(struct fw_card *card,
                       struct fw_request *request, int rcode);