6 files changed, 51 insertions, 13 deletions

diff --git a/drivers/misc/ibmasm/command.c b/drivers/misc/ibmasm/command.c
index 245b0058381d..07a085ccbd5b 100644
--- a/drivers/misc/ibmasm/command.c
+++ b/drivers/misc/ibmasm/command.c
@@ -23,6 +23,7 @@
  */
 
 #include "ibmasm.h"
+#include "lowlevel.h"
 
 static void exec_next_command(struct service_processor *sp);
 static void free_command(struct kobject *kobj);
@@ -31,8 +32,9 @@ static struct kobj_type ibmasm_cmd_kobj_type = {
         .release = free_command,
 };
 
+static atomic_t command_count = ATOMIC_INIT(0);
 
-struct command *ibmasm_new_command(size_t buffer_size)
+struct command *ibmasm_new_command(struct service_processor *sp, size_t buffer_size)
 {
         struct command *cmd;
 
@@ -55,11 +57,15 @@ struct command *ibmasm_new_command(size_t buffer_size)
 
         kobject_init(&cmd->kobj);
         cmd->kobj.ktype = &ibmasm_cmd_kobj_type;
+        cmd->lock = &sp->lock;
 
         cmd->status = IBMASM_CMD_PENDING;
         init_waitqueue_head(&cmd->wait);
         INIT_LIST_HEAD(&cmd->queue_node);
 
+        atomic_inc(&command_count);
+        dbg("command count: %d\n", atomic_read(&command_count));
+
         return cmd;
 }
 
@@ -68,6 +74,8 @@ static void free_command(struct kobject *kobj)
         struct command *cmd = to_command(kobj);
  
         list_del(&cmd->queue_node);
+        atomic_dec(&command_count);
+        dbg("command count: %d\n", atomic_read(&command_count));
         kfree(cmd->buffer);
         kfree(cmd);
 }
@@ -94,8 +102,14 @@ static struct command *dequeue_command(struct service_processor *sp)
 
 static inline void do_exec_command(struct service_processor *sp)
 {
+        char tsbuf[32];
+
+        dbg("%s:%d at %s\n", __FUNCTION__, __LINE__, get_timestamp(tsbuf));
+
         if (ibmasm_send_i2o_message(sp)) {
                 sp->current_command->status = IBMASM_CMD_FAILED;
+                wake_up(&sp->current_command->wait);
+                command_put(sp->current_command);
                 exec_next_command(sp);
         }
 }
@@ -111,14 +125,16 @@ static inline void do_exec_command(struct service_processor *sp)
 void ibmasm_exec_command(struct service_processor *sp, struct command *cmd)
 {
         unsigned long flags;
+        char tsbuf[32];
+
+        dbg("%s:%d at %s\n", __FUNCTION__, __LINE__, get_timestamp(tsbuf));
 
         spin_lock_irqsave(&sp->lock, flags);
 
         if (!sp->current_command) {
-                command_get(cmd);
                 sp->current_command = cmd;
+                command_get(sp->current_command);
                 spin_unlock_irqrestore(&sp->lock, flags);
-
                 do_exec_command(sp);
         } else {
                 enqueue_command(sp, cmd);
@@ -129,9 +145,9 @@ void ibmasm_exec_command(struct service_processor *sp, struct command *cmd)
 static void exec_next_command(struct service_processor *sp)
 {
         unsigned long flags;
+        char tsbuf[32];
 
-        wake_up(&sp->current_command->wait);
-        command_put(sp->current_command);
+        dbg("%s:%d at %s\n", __FUNCTION__, __LINE__, get_timestamp(tsbuf));
 
         spin_lock_irqsave(&sp->lock, flags);
         sp->current_command = dequeue_command(sp);
@@ -169,7 +185,9 @@ void ibmasm_receive_command_response(struct service_processor *sp, void *respons
         if (!sp->current_command) 
                 return; 
 
-        memcpy(cmd->buffer, response, min(size, cmd->buffer_size));
+        memcpy_fromio(cmd->buffer, response, min(size, cmd->buffer_size));
         cmd->status = IBMASM_CMD_COMPLETE;
+        wake_up(&sp->current_command->wait);
+        command_put(sp->current_command);
         exec_next_command(sp);
 }
diff --git a/drivers/misc/ibmasm/dot_command.c b/drivers/misc/ibmasm/dot_command.c
index 478a8d898fc1..13c52f866e2e 100644
--- a/drivers/misc/ibmasm/dot_command.c
+++ b/drivers/misc/ibmasm/dot_command.c
@@ -33,7 +33,13 @@ void ibmasm_receive_message(struct service_processor *sp, void *message, int mes
         u32 size;
         struct dot_command_header *header = (struct dot_command_header *)message;
 
+        if (message_size == 0)
+                return;
+
         size = get_dot_command_size(message);
+        if (size == 0)
+                return;
+
         if (size > message_size)
                 size = message_size;
 
@@ -67,7 +73,7 @@ int ibmasm_send_driver_vpd(struct service_processor *sp)
         u8 *vpd_data;
         int result = 0;
 
-        command = ibmasm_new_command(INIT_BUFFER_SIZE);
+        command = ibmasm_new_command(sp, INIT_BUFFER_SIZE);
         if (command == NULL)
                 return -ENOMEM;
 
@@ -121,7 +127,7 @@ int ibmasm_send_os_state(struct service_processor *sp, int os_state)
         struct os_state_command *os_state_cmd;
         int result = 0;
 
-        cmd = ibmasm_new_command(sizeof(struct os_state_command));
+        cmd = ibmasm_new_command(sp, sizeof(struct os_state_command));
         if (cmd == NULL)
                 return -ENOMEM;
 
diff --git a/drivers/misc/ibmasm/heartbeat.c b/drivers/misc/ibmasm/heartbeat.c
index ce09309174d6..f295401fac21 100644
--- a/drivers/misc/ibmasm/heartbeat.c
+++ b/drivers/misc/ibmasm/heartbeat.c
@@ -25,6 +25,7 @@
 #include <linux/notifier.h>
 #include "ibmasm.h"
 #include "dot_command.h"
+#include "lowlevel.h"
 
 static int suspend_heartbeats = 0;
 
@@ -62,7 +63,7 @@ void ibmasm_unregister_panic_notifier(void)
 
 int ibmasm_heartbeat_init(struct service_processor *sp)
 {
-        sp->heartbeat = ibmasm_new_command(HEARTBEAT_BUFFER_SIZE);
+        sp->heartbeat = ibmasm_new_command(sp, HEARTBEAT_BUFFER_SIZE);
         if (sp->heartbeat == NULL)
                 return -ENOMEM;
 
@@ -71,6 +72,12 @@ int ibmasm_heartbeat_init(struct service_processor *sp)
 
 void ibmasm_heartbeat_exit(struct service_processor *sp)
 {
+        char tsbuf[32];
+
+        dbg("%s:%d at %s\n", __FUNCTION__, __LINE__, get_timestamp(tsbuf));
+        ibmasm_wait_for_response(sp->heartbeat, IBMASM_CMD_TIMEOUT_NORMAL);
+        dbg("%s:%d at %s\n", __FUNCTION__, __LINE__, get_timestamp(tsbuf));
+        suspend_heartbeats = 1;
         command_put(sp->heartbeat);
 }
 
@@ -78,14 +85,16 @@ void ibmasm_receive_heartbeat(struct service_processor *sp,  void *message, size
 {
         struct command *cmd = sp->heartbeat;
         struct dot_command_header *header = (struct dot_command_header *)cmd->buffer;
+        char tsbuf[32];
 
+        dbg("%s:%d at %s\n", __FUNCTION__, __LINE__, get_timestamp(tsbuf));
         if (suspend_heartbeats)
                 return;
 
         /* return the received dot command to sender */
         cmd->status = IBMASM_CMD_PENDING;
         size = min(size, cmd->buffer_size);
-        memcpy(cmd->buffer, message, size);
+        memcpy_fromio(cmd->buffer, message, size);
         header->type = sp_write;
         ibmasm_exec_command(sp, cmd);
 }
diff --git a/drivers/misc/ibmasm/ibmasm.h b/drivers/misc/ibmasm/ibmasm.h
index 653a7d096a8b..ecce4ffd3e23 100644
--- a/drivers/misc/ibmasm/ibmasm.h
+++ b/drivers/misc/ibmasm/ibmasm.h
@@ -95,12 +95,17 @@ struct command {
         size_t                  buffer_size;
         int                     status;
         struct kobject          kobj;
+        spinlock_t              *lock;
 };
 #define to_command(c) container_of(c, struct command, kobj)
 
 static inline void command_put(struct command *cmd)
 {
+        unsigned long flags;
+
+        spin_lock_irqsave(cmd->lock, flags);
         kobject_put(&cmd->kobj);
+        spin_unlock_irqrestore(cmd->lock, flags);
 }
 
 static inline void command_get(struct command *cmd)
@@ -159,7 +164,7 @@ struct service_processor {
 };
 
 /* command processing */
-extern struct command *ibmasm_new_command(size_t buffer_size);
+extern struct command *ibmasm_new_command(struct service_processor *sp, size_t buffer_size);
 extern void ibmasm_exec_command(struct service_processor *sp, struct command *cmd);
 extern void ibmasm_wait_for_response(struct command *cmd, int timeout);
 extern void ibmasm_receive_command_response(struct service_processor *sp, void *response,  size_t size);
diff --git a/drivers/misc/ibmasm/ibmasmfs.c b/drivers/misc/ibmasm/ibmasmfs.c
index ca839162e4f7..5c550fcac2c4 100644
--- a/drivers/misc/ibmasm/ibmasmfs.c
+++ b/drivers/misc/ibmasm/ibmasmfs.c
@@ -321,7 +321,7 @@ static ssize_t command_file_write(struct file *file, const char __user *ubuff, s
         if (command_data->command)
                 return -EAGAIN;
 
-        cmd = ibmasm_new_command(count);
+        cmd = ibmasm_new_command(command_data->sp, count);
         if (!cmd)
                 return -ENOMEM;
 
diff --git a/drivers/misc/ibmasm/r_heartbeat.c b/drivers/misc/ibmasm/r_heartbeat.c
index 93d9c1b2ad6f..f8fdb2d5417e 100644
--- a/drivers/misc/ibmasm/r_heartbeat.c
+++ b/drivers/misc/ibmasm/r_heartbeat.c
@@ -63,7 +63,7 @@ int ibmasm_start_reverse_heartbeat(struct service_processor *sp, struct reverse_
         int times_failed = 0;
         int result = 1;
 
-        cmd = ibmasm_new_command(sizeof rhb_dot_cmd);
+        cmd = ibmasm_new_command(sp, sizeof rhb_dot_cmd);
         if (!cmd)
                 return -ENOMEM;