5 files changed, 77 insertions, 7 deletions

diff --git a/drivers/firewire/core-card.c b/drivers/firewire/core-card.c
index 0c312c4bb4bd..6d1cfae6aad4 100644
--- a/drivers/firewire/core-card.c
+++ b/drivers/firewire/core-card.c
@@ -500,6 +500,7 @@ void fw_card_initialize(struct fw_card *card,
         kref_init(&card->kref);
         init_completion(&card->done);
         INIT_LIST_HEAD(&card->transaction_list);
+        INIT_LIST_HEAD(&card->phy_receiver_list);
         spin_lock_init(&card->lock);
 
         card->local_node = NULL;
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index f95719926487..0425dd5dfcd3 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -69,6 +69,9 @@ struct client {
         struct fw_iso_buffer buffer;
         unsigned long vm_start;
 
+        struct list_head phy_receiver_link;
+        u64 phy_receiver_closure;
+
         struct list_head link;
         struct kref kref;
 };
@@ -201,6 +204,11 @@ struct outbound_phy_packet_event {
         struct fw_cdev_event_phy_packet phy_packet;
 };
 
+struct inbound_phy_packet_event {
+        struct event event;
+        struct fw_cdev_event_phy_packet phy_packet;
+};
+
 static inline void __user *u64_to_uptr(__u64 value)
 {
         return (void __user *)(unsigned long)value;
@@ -236,6 +244,7 @@ static int fw_device_op_open(struct inode *inode, struct file *file)
         idr_init(&client->resource_idr);
         INIT_LIST_HEAD(&client->event_list);
         init_waitqueue_head(&client->wait);
+        INIT_LIST_HEAD(&client->phy_receiver_link);
         kref_init(&client->kref);
 
         file->private_data = client;
@@ -357,7 +366,7 @@ static void queue_bus_reset_event(struct client *client)
 
         e = kzalloc(sizeof(*e), GFP_KERNEL);
         if (e == NULL) {
-                fw_notify("Out of memory when allocating bus reset event\n");
+                fw_notify("Out of memory when allocating event\n");
                 return;
         }
 
@@ -404,6 +413,7 @@ union ioctl_arg {
         struct fw_cdev_send_stream_packet       send_stream_packet;
         struct fw_cdev_get_cycle_timer2         get_cycle_timer2;
         struct fw_cdev_send_phy_packet          send_phy_packet;
+        struct fw_cdev_receive_phy_packets      receive_phy_packets;
 };
 
 static int ioctl_get_info(struct client *client, union ioctl_arg *arg)
@@ -671,9 +681,10 @@ static void handle_request(struct fw_card *card, struct fw_request *request,
 
         r = kmalloc(sizeof(*r), GFP_ATOMIC);
         e = kmalloc(sizeof(*e), GFP_ATOMIC);
-        if (r == NULL || e == NULL)
+        if (r == NULL || e == NULL) {
+                fw_notify("Out of memory when allocating event\n");
                 goto failed;
-
+        }
         r->card    = card;
         r->request = request;
         r->data    = payload;
@@ -902,9 +913,10 @@ static void iso_callback(struct fw_iso_context *context, u32 cycle,
         struct iso_interrupt_event *e;
 
         e = kmalloc(sizeof(*e) + header_length, GFP_ATOMIC);
-        if (e == NULL)
+        if (e == NULL) {
+                fw_notify("Out of memory when allocating event\n");
                 return;
-
+        }
         e->interrupt.type      = FW_CDEV_EVENT_ISO_INTERRUPT;
         e->interrupt.closure   = client->iso_closure;
         e->interrupt.cycle     = cycle;
@@ -1447,6 +1459,52 @@ static int ioctl_send_phy_packet(struct client *client, union ioctl_arg *arg)
         return 0;
 }
 
+static int ioctl_receive_phy_packets(struct client *client, union ioctl_arg *arg)
+{
+        struct fw_cdev_receive_phy_packets *a = &arg->receive_phy_packets;
+        struct fw_card *card = client->device->card;
+
+        /* Access policy: Allow this ioctl only on local nodes' device files. */
+        if (!client->device->is_local)
+                return -ENOSYS;
+
+        spin_lock_irq(&card->lock);
+
+        list_move_tail(&client->phy_receiver_link, &card->phy_receiver_list);
+        client->phy_receiver_closure = a->closure;
+
+        spin_unlock_irq(&card->lock);
+
+        return 0;
+}
+
+void fw_cdev_handle_phy_packet(struct fw_card *card, struct fw_packet *p)
+{
+        struct client *client;
+        struct inbound_phy_packet_event *e;
+        unsigned long flags;
+
+        spin_lock_irqsave(&card->lock, flags);
+
+        list_for_each_entry(client, &card->phy_receiver_list, phy_receiver_link) {
+                e = kmalloc(sizeof(*e) + 8, GFP_ATOMIC);
+                if (e == NULL) {
+                        fw_notify("Out of memory when allocating event\n");
+                        break;
+                }
+                e->phy_packet.closure   = client->phy_receiver_closure;
+                e->phy_packet.type      = FW_CDEV_EVENT_PHY_PACKET_RECEIVED;
+                e->phy_packet.rcode     = RCODE_COMPLETE;
+                e->phy_packet.length    = 8;
+                e->phy_packet.data[0]   = p->header[1];
+                e->phy_packet.data[1]   = p->header[2];
+                queue_event(client, &e->event,
+                            &e->phy_packet, sizeof(e->phy_packet) + 8, NULL, 0);
+        }
+
+        spin_unlock_irqrestore(&card->lock, flags);
+}
+
 static int (* const ioctl_handlers[])(struct client *, union ioctl_arg *) = {
         [0x00] = ioctl_get_info,
         [0x01] = ioctl_send_request,
@@ -1470,6 +1528,7 @@ static int (* const ioctl_handlers[])(struct client *, union ioctl_arg *) = {
         [0x13] = ioctl_send_stream_packet,
         [0x14] = ioctl_get_cycle_timer2,
         [0x15] = ioctl_send_phy_packet,
+        [0x16] = ioctl_receive_phy_packets,
 };
 
 static int dispatch_ioctl(struct client *client,
@@ -1577,6 +1636,10 @@ static int fw_device_op_release(struct inode *inode, struct file *file)
         struct client *client = file->private_data;
         struct event *event, *next_event;
 
+        spin_lock_irq(&client->device->card->lock);
+        list_del(&client->phy_receiver_link);
+        spin_unlock_irq(&client->device->card->lock);
+
         mutex_lock(&client->device->client_list_mutex);
         list_del(&client->link);
         mutex_unlock(&client->device->client_list_mutex);
diff --git a/drivers/firewire/core-transaction.c b/drivers/firewire/core-transaction.c
index e2e4dc624fb6..6f225cacbc3d 100644
--- a/drivers/firewire/core-transaction.c
+++ b/drivers/firewire/core-transaction.c
@@ -883,6 +883,11 @@ void fw_core_handle_request(struct fw_card *card, struct fw_packet *p)
         if (p->ack != ACK_PENDING && p->ack != ACK_COMPLETE)
                 return;
 
+        if (TCODE_IS_LINK_INTERNAL(HEADER_GET_TCODE(p->header[0]))) {
+                fw_cdev_handle_phy_packet(card, p);
+                return;
+        }
+
         request = allocate_request(card, p);
         if (request == NULL) {
                 /* FIXME: send statically allocated busy packet. */
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index ff6c90922001..3102b6b63438 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -128,6 +128,7 @@ extern const struct file_operations fw_device_ops;
 
 void fw_device_cdev_update(struct fw_device *device);
 void fw_device_cdev_remove(struct fw_device *device);
+void fw_cdev_handle_phy_packet(struct fw_card *card, struct fw_packet *p);
 
 
 /* -device */
@@ -214,6 +215,7 @@ static inline bool is_next_generation(int new_generation, int old_generation)
 
 #define TCODE_IS_READ_REQUEST(tcode)    (((tcode) & ~1) == 4)
 #define TCODE_IS_BLOCK_PACKET(tcode)    (((tcode) &  1) != 0)
+#define TCODE_IS_LINK_INTERNAL(tcode)   ((tcode) == 0xe)
 #define TCODE_IS_REQUEST(tcode)         (((tcode) &  2) == 0)
 #define TCODE_IS_RESPONSE(tcode)        (((tcode) &  2) != 0)
 #define TCODE_HAS_REQUEST_DATA(tcode)   (((tcode) & 12) != 4)
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index bb6a92bc9e6a..08afccc66333 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -1759,10 +1759,9 @@ static int ohci_enable(struct fw_card *card,
                   OHCI1394_HCControl_noByteSwapData);
 
         reg_write(ohci, OHCI1394_SelfIDBuffer, ohci->self_id_bus);
-        reg_write(ohci, OHCI1394_LinkControlClear,
-                  OHCI1394_LinkControl_rcvPhyPkt);
         reg_write(ohci, OHCI1394_LinkControlSet,
                   OHCI1394_LinkControl_rcvSelfID |
+                  OHCI1394_LinkControl_rcvPhyPkt |
                   OHCI1394_LinkControl_cycleTimerEnable |
                   OHCI1394_LinkControl_cycleMaster);