[PATCH v5 6/8] usb: mausb_host: Add logic for PAL-to-PAL communication

[vladimir . stankovic]

Implemeneted connection between PAL and Link layers and set up
environment for exchanging PAL-to-PAL messages.

Within this patch, driver's sysfs parameters have been created
with intention to configure remote connection parameters.

Signed-off-by: Vladimir Stankovic <vladimir.stankovic@xxxxxxxxxxxxxxx>
---
 drivers/usb/mausb_host/hpal.c       | 522 ++++++++++++++++++++++++++++
 drivers/usb/mausb_host/hpal.h       |   6 +
 drivers/usb/mausb_host/mausb_core.c | 103 +++++-
 3 files changed, 630 insertions(+), 1 deletion(-)

diff --git a/drivers/usb/mausb_host/hpal.c b/drivers/usb/mausb_host/hpal.c
index e10b98002afd..74bebc03d7e6 100644
--- a/drivers/usb/mausb_host/hpal.c
+++ b/drivers/usb/mausb_host/hpal.c
@@ -13,10 +13,15 @@
 #include "hcd.h"
 #include "utils.h"
 
+#define MAUSB_DELETE_MADEV_TIMEOUT_MS 3000
+
 struct mss mss;
 
+static int mausb_start_connection_timer(struct mausb_device *dev);
 static int mausb_power_state_cb(struct notifier_block *nb, unsigned long action,
 				void *data);
+static void mausb_signal_empty_mss(void);
+static void mausb_remove_madev_from_list(u8 madev_addr);
 static void mausb_execute_urb_dequeue(struct work_struct *dequeue_work);
 static int mausb_start_heartbeat_timer(void);
 
@@ -177,6 +182,55 @@ static bool mausb_return_urb_ctx_to_tree(struct mausb_urb_ctx *urb_ctx,
 	return true;
 }
 
+static void mausb_complete_urbs_from_tree(void)
+{
+	struct mausb_urb_ctx *urb_ctx = NULL;
+	struct urb	     *current_urb = NULL;
+	struct rb_node	     *current_node = NULL;
+	unsigned long flags;
+	int status = 0;
+	int ret;
+
+	mausb_pr_debug("Completing all urbs from tree");
+
+	spin_lock_irqsave(&mhcd->lock, flags);
+
+	while ((current_node = rb_first(&mhcd->mausb_urbs))) {
+		urb_ctx = rb_entry(current_node, struct mausb_urb_ctx, rb_node);
+
+		current_urb = urb_ctx->urb;
+		mausb_delete_urb_ctx_from_tree(urb_ctx);
+		mausb_uninit_data_iterator(&urb_ctx->iterator);
+		kfree(urb_ctx);
+
+		ret = usb_hcd_check_unlink_urb(current_urb->hcpriv,
+					       current_urb, status);
+		if (ret == -EIDRM)
+			mausb_pr_warn("Urb=%p is already unlinked",
+				      current_urb);
+		else
+			usb_hcd_unlink_urb_from_ep(current_urb->hcpriv,
+						   current_urb);
+
+		spin_unlock_irqrestore(&mhcd->lock, flags);
+
+		/* Prepare urb for completion */
+		mausb_pr_debug("Completing urb=%p", current_urb);
+
+		current_urb->status	   = -EPROTO;
+		current_urb->actual_length = 0;
+		atomic_dec(&current_urb->use_count);
+		usb_hcd_giveback_urb(current_urb->hcpriv, current_urb,
+				     current_urb->status);
+
+		spin_lock_irqsave(&mhcd->lock, flags);
+	}
+
+	spin_unlock_irqrestore(&mhcd->lock, flags);
+
+	mausb_pr_debug("Completed all urbs from tree");
+}
+
 /*After this function call only valid thing to do with urb is to give it back*/
 struct mausb_urb_ctx *mausb_unlink_and_delete_urb_from_tree(struct urb *urb,
 							    int status)
@@ -284,6 +338,161 @@ static inline void mausb_port_has_changed_event(struct mausb_device *dev,
 		mausb_port_has_changed(USB20HUB, HIGH_SPEED, dev);
 }
 
+static void mausb_socket_disconnect_event(struct work_struct *work)
+{
+	struct mausb_device *dev = container_of(work, struct mausb_device,
+						socket_disconnect_work);
+	struct mausb_event event;
+
+	mausb_pr_info("madev_addr=%d", dev->madev_addr);
+
+	mausb_ip_disconnect(dev->ctrl_channel);
+	mausb_destroy_ip_ctx(dev->ctrl_channel);
+	dev->ctrl_channel = NULL;
+
+	mausb_ip_disconnect(dev->bulk_channel);
+	mausb_destroy_ip_ctx(dev->bulk_channel);
+	dev->bulk_channel = NULL;
+
+	mausb_ip_disconnect(dev->isoch_channel);
+	mausb_destroy_ip_ctx(dev->isoch_channel);
+	dev->isoch_channel = NULL;
+
+	if (dev->mgmt_channel) {
+		memset(&event, 0, sizeof(event));
+		event.type = MAUSB_EVENT_TYPE_NETWORK_DISCONNECTED;
+		event.data.device_id = dev->id;
+
+		mausb_pr_info("Releasing MAUSB device ref");
+		kref_put(&dev->refcount, mausb_release_ma_dev_async);
+	}
+
+	mausb_ip_disconnect(dev->mgmt_channel);
+	mausb_destroy_ip_ctx(dev->mgmt_channel);
+	dev->mgmt_channel = NULL;
+
+	memset(dev->channel_map, 0, sizeof(dev->channel_map));
+}
+
+static void mausb_disconnect_ma_dev(struct mausb_device *dev)
+{
+	mausb_pr_info("Disconnecting MAUSB device madev_addr=%d",
+		      dev->madev_addr);
+
+	if (!dev->dev_connected) {
+		mausb_pr_warn("MAUSB device is not connected");
+		kref_put(&dev->refcount, mausb_release_ma_dev_async);
+		return;
+	}
+	mausb_hcd_disconnect(dev->port_number, dev->dev_type, dev->dev_speed);
+
+	if (dev->dev_type == USB30HUB)
+		mausb_hcd_disconnect(dev->port_number, USB20HUB, HIGH_SPEED);
+}
+
+static void mausb_hcd_disconnect_event(struct work_struct *work)
+{
+	struct mausb_device *ma_dev = container_of(work, struct mausb_device,
+						   hcd_disconnect_work);
+
+	mausb_disconnect_ma_dev(ma_dev);
+}
+
+static void mausb_delete_madev(struct work_struct *work)
+{
+	struct mausb_device *dev = container_of(work, struct mausb_device,
+						madev_delete_work);
+	struct mausb_event	event;
+	struct completion	completion;
+	struct completion	*user_event;
+	struct mausb_completion mausb_completion;
+	long status;
+	unsigned long timeout = msecs_to_jiffies(MAUSB_DELETE_MADEV_TIMEOUT_MS);
+
+	mausb_pr_info("Deleting MAUSB device madev_addr=%d", dev->madev_addr);
+
+	del_timer_sync(&dev->connection_timer);
+
+	/* Client IS responsive */
+	if (!atomic_read(&dev->unresponsive_client)) {
+		memset(&event, 0, sizeof(event));
+		event.type = MAUSB_EVENT_TYPE_DELETE_MA_DEV;
+		event.mgmt.delete_ma_dev.device_id = dev->id;
+		event.mgmt.delete_ma_dev.event_id  = mausb_event_id(dev);
+
+		init_completion(&completion);
+		mausb_completion.completion_event = &completion;
+		mausb_completion.event_id = event.mgmt.delete_ma_dev.event_id;
+		mausb_completion.mausb_event = &event;
+
+		mausb_insert_event(dev, &mausb_completion);
+
+		mausb_pr_debug("Deleting MAUSB device...");
+
+		status = wait_for_completion_interruptible_timeout(&completion,
+								   timeout);
+
+		mausb_pr_debug("Deleting MAUSB device event finished with %ld",
+			       status);
+
+		mausb_remove_event(dev, &mausb_completion);
+
+		user_event = &dev->user_finished_event;
+
+		status = wait_for_completion_interruptible_timeout(user_event,
+								   timeout);
+		mausb_pr_info("User event finished with %ld", status);
+	}
+
+	flush_workqueue(dev->workq);
+	destroy_workqueue(dev->workq);
+
+	mausb_clear_hcd_madev(dev->port_number);
+
+	mausb_remove_madev_from_list(dev->madev_addr);
+
+	put_net(dev->net_ns);
+
+	kfree(dev);
+	mausb_signal_empty_mss();
+
+	mausb_pr_info("MAUSB device deleted. Version=%s", MAUSB_DRIVER_VERSION);
+}
+
+static void mausb_ping_work(struct work_struct *work)
+{
+	struct mausb_device *dev = container_of(work, struct mausb_device,
+						ping_work);
+
+	if (mausb_start_connection_timer(dev) < 0) {
+		mausb_pr_err("Device disconnecting due to session timeout madev_addr=%d",
+			     dev->madev_addr);
+		queue_work(dev->workq, &dev->socket_disconnect_work);
+		queue_work(dev->workq, &dev->hcd_disconnect_work);
+		return;
+	}
+}
+
+static void mausb_heartbeat_work(struct work_struct *work)
+{
+	struct mausb_device *dev = container_of(work, struct mausb_device,
+						heartbeat_work);
+
+	mausb_pr_err("Device disconnecting - app is unresponsive");
+	atomic_set(&dev->unresponsive_client, 1);
+	mausb_complete_urbs_from_tree();
+	queue_work(dev->workq, &dev->socket_disconnect_work);
+	queue_work(dev->workq, &dev->hcd_disconnect_work);
+}
+
+static void mausb_connection_timer_func(struct timer_list *timer)
+{
+	struct mausb_device *dev = container_of(timer, struct mausb_device,
+						connection_timer);
+
+	queue_work(dev->workq, &dev->ping_work);
+}
+
 static void mausb_heartbeat_timer_func(struct timer_list *timer)
 {
 	unsigned long flags = 0;
@@ -308,6 +517,99 @@ static void mausb_heartbeat_timer_func(struct timer_list *timer)
 	}
 }
 
+static struct mausb_device *
+mausb_create_madev(struct mausb_device_address dev_addr, u8 madev_address,
+		   int *status)
+{
+	struct mausb_device *dev;
+	unsigned long flags = 0;
+	char workq_name[16];
+	struct workqueue_struct *workq;
+
+	memset(workq_name, 0, sizeof(workq_name));
+	sprintf(workq_name, "%x", madev_address);
+	strcat(workq_name, "_madev_workq");
+
+	mausb_pr_debug("madev_workq_name = %s", workq_name);
+
+	workq = alloc_ordered_workqueue(workq_name, WQ_MEM_RECLAIM);
+	if (!workq) {
+		mausb_pr_alert("Could not allocate workqueue!");
+		*status = -ENOMEM;
+		return NULL;
+	}
+
+	spin_lock_irqsave(&mss.lock, flags);
+
+	if (mss.deinit_in_progress) {
+		spin_unlock_irqrestore(&mss.lock, flags);
+		mausb_pr_alert("Device creating failed - mss deinit in progress");
+		flush_workqueue(workq);
+		destroy_workqueue(workq);
+		*status = -ESHUTDOWN;
+		return NULL;
+	}
+
+	dev = mausb_get_dev_from_addr_unsafe(madev_address);
+	if (dev) {
+		spin_unlock_irqrestore(&mss.lock, flags);
+		mausb_pr_debug("MAUSB device already connected, madev_address=%x",
+			       madev_address);
+		flush_workqueue(workq);
+		destroy_workqueue(workq);
+		*status = -EEXIST;
+		return NULL;
+	}
+
+	dev = kzalloc(sizeof(*dev), GFP_ATOMIC);
+
+	if (!dev) {
+		spin_unlock_irqrestore(&mss.lock, flags);
+		mausb_pr_alert("Could not allocate MAUSB device!");
+		flush_workqueue(workq);
+		destroy_workqueue(workq);
+		*status = -ENOMEM;
+		return NULL;
+	}
+
+	mausb_pr_info("Create MAUSB device. Version=%s", MAUSB_DRIVER_VERSION);
+
+	dev->workq = workq;
+
+	INIT_WORK(&dev->socket_disconnect_work, mausb_socket_disconnect_event);
+	INIT_WORK(&dev->hcd_disconnect_work, mausb_hcd_disconnect_event);
+	INIT_WORK(&dev->madev_delete_work, mausb_delete_madev);
+	INIT_WORK(&dev->ping_work, mausb_ping_work);
+	INIT_WORK(&dev->heartbeat_work, mausb_heartbeat_work);
+
+	kref_init(&dev->refcount);
+
+	dev->event_id = 0;
+	spin_lock_init(&dev->event_id_lock);
+
+	INIT_LIST_HEAD(&dev->completion_events);
+	spin_lock_init(&dev->completion_events_lock);
+	spin_lock_init(&dev->num_of_user_events_lock);
+	spin_lock_init(&dev->connection_timer_lock);
+
+	init_completion(&dev->user_finished_event);
+	atomic_set(&dev->unresponsive_client, 0);
+
+	timer_setup(&dev->connection_timer, mausb_connection_timer_func, 0);
+
+	dev->dev_addr = dev_addr;
+	dev->madev_addr = madev_address;
+	dev->net_ns = get_net(current->nsproxy->net_ns);
+
+	list_add_tail(&dev->list_entry, &mss.madev_list);
+
+	reinit_completion(&mss.empty);
+
+	spin_unlock_irqrestore(&mss.lock, flags);
+
+	return dev;
+}
+
 void mausb_release_ma_dev_async(struct kref *kref)
 {
 	struct mausb_device *dev = container_of(kref, struct mausb_device,
@@ -318,6 +620,45 @@ void mausb_release_ma_dev_async(struct kref *kref)
 	schedule_work(&dev->madev_delete_work);
 }
 
+int mausb_initiate_dev_connection(struct mausb_device_address dev_addr,
+				  u8 madev_address)
+{
+	int error = 0;
+	struct mausb_device *dev;
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&mss.lock, flags);
+	dev = mausb_get_dev_from_addr_unsafe(madev_address);
+	spin_unlock_irqrestore(&mss.lock, flags);
+
+	if (dev) {
+		mausb_pr_debug("MAUSB device already connected, madev_address=%x",
+			       madev_address);
+		return -EEXIST;
+	}
+
+	dev = mausb_create_madev(dev_addr, madev_address, &error);
+
+	if (!dev)
+		return error;
+
+	mausb_pr_info("New MAUSB device created madev_addr=%d", madev_address);
+
+	error = mausb_init_ip_ctx(&dev->mgmt_channel, dev->net_ns,
+				  dev->dev_addr.ip.address,
+				  dev->dev_addr.ip.port.management, dev,
+				  mausb_ip_callback, MAUSB_MGMT_CHANNEL);
+	if (error) {
+		mausb_pr_err("Mgmt ip context init failed: error=%d", error);
+		kref_put(&dev->refcount, mausb_release_ma_dev_async);
+		return error;
+	}
+
+	mausb_ip_connect_async(dev->mgmt_channel);
+
+	return 0;
+}
+
 int mausb_enqueue_event_from_user(u8 madev_addr, u16 num_of_events,
 				  u16 num_of_completed)
 {
@@ -418,6 +759,26 @@ int mausb_signal_event(struct mausb_device *dev,
 	return -ETIMEDOUT;
 }
 
+static int mausb_start_connection_timer(struct mausb_device *dev)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&dev->connection_timer_lock, flags);
+
+	if (++dev->receive_failures_num > MAUSB_MAX_RECEIVE_FAILURES) {
+		mausb_pr_err("Missed more than %d ping responses",
+			     MAUSB_MAX_RECEIVE_FAILURES);
+		spin_unlock_irqrestore(&dev->connection_timer_lock, flags);
+		return -ETIMEDOUT;
+	}
+
+	mod_timer(&dev->connection_timer, jiffies + msecs_to_jiffies(1000));
+
+	spin_unlock_irqrestore(&dev->connection_timer_lock, flags);
+
+	return 0;
+}
+
 void mausb_reset_connection_timer(struct mausb_device *dev)
 {
 	unsigned long flags = 0;
@@ -661,6 +1022,36 @@ struct mausb_device *mausb_get_dev_from_addr_unsafe(u8 madev_addr)
 	return NULL;
 }
 
+static void mausb_remove_madev_from_list(u8 madev_addr)
+{
+	unsigned long flags = 0;
+	struct mausb_device *ma_dev, *tmp = NULL;
+
+	spin_lock_irqsave(&mss.lock, flags);
+
+	list_for_each_entry_safe(ma_dev, tmp, &mss.madev_list, list_entry) {
+		if (ma_dev->madev_addr == madev_addr) {
+			list_del(&ma_dev->list_entry);
+			break;
+		}
+	}
+
+	if (list_empty(&mss.madev_list))
+		reinit_completion(&mss.rings_events.mausb_ring_has_events);
+
+	spin_unlock_irqrestore(&mss.lock, flags);
+}
+
+static void mausb_signal_empty_mss(void)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&mss.lock, flags);
+	if (list_empty(&mss.madev_list))
+		complete(&mss.empty);
+	spin_unlock_irqrestore(&mss.lock, flags);
+}
+
 static inline
 struct mausb_ip_ctx *mausb_get_data_channel(struct mausb_device *ma_dev,
 					    enum mausb_channel channel)
@@ -814,6 +1205,137 @@ void mausb_cleanup_chunks_list(struct list_head *chunks_list)
 	}
 }
 
+static void mausb_init_ip_ctx_helper(struct mausb_device *dev,
+				     struct mausb_ip_ctx **ip_ctx,
+				     u16 port,
+				     enum mausb_channel channel)
+{
+	int status = mausb_init_ip_ctx(ip_ctx, dev->net_ns,
+				       dev->dev_addr.ip.address, port, dev,
+				       mausb_ip_callback, channel);
+	if (status < 0) {
+		mausb_pr_err("Init ip context failed with error=%d", status);
+		queue_work(dev->workq, &dev->socket_disconnect_work);
+		return;
+	}
+
+	dev->channel_map[channel] = *ip_ctx;
+	mausb_ip_connect_async(*ip_ctx);
+}
+
+static void mausb_connect_callback(struct mausb_device *dev,
+				   enum mausb_channel channel, int status)
+{
+	struct mausb_device_address *dev_addr = &dev->dev_addr;
+
+	mausb_pr_info("Connect callback for channel=%d with status=%d",
+		      channel, status);
+
+	if (status < 0) {
+		queue_work(dev->workq, &dev->socket_disconnect_work);
+		return;
+	}
+
+	if (channel == MAUSB_MGMT_CHANNEL) {
+		if (dev_addr->ip.port.control == 0) {
+			dev->channel_map[MAUSB_CTRL_CHANNEL] =
+				dev->mgmt_channel;
+			channel = MAUSB_CTRL_CHANNEL;
+		} else {
+			mausb_init_ip_ctx_helper(dev, &dev->ctrl_channel,
+						 dev_addr->ip.port.control,
+						 MAUSB_CTRL_CHANNEL);
+			return;
+		}
+	}
+
+	if (channel == MAUSB_CTRL_CHANNEL) {
+		if (dev_addr->ip.port.bulk == 0) {
+			dev->channel_map[MAUSB_BULK_CHANNEL] =
+				dev->channel_map[MAUSB_CTRL_CHANNEL];
+			channel = MAUSB_BULK_CHANNEL;
+		} else {
+			mausb_init_ip_ctx_helper(dev, &dev->bulk_channel,
+						 dev_addr->ip.port.bulk,
+						 MAUSB_BULK_CHANNEL);
+			return;
+		}
+	}
+
+	if (channel == MAUSB_BULK_CHANNEL) {
+		if (dev_addr->ip.port.isochronous == 0) {
+			/* if there is no isoch port use tcp for it */
+			dev->channel_map[MAUSB_ISOCH_CHANNEL] =
+				dev->channel_map[MAUSB_BULK_CHANNEL];
+			channel = MAUSB_ISOCH_CHANNEL;
+		} else {
+			mausb_init_ip_ctx_helper(dev, &dev->isoch_channel,
+						 dev_addr->ip.port.isochronous,
+						 MAUSB_ISOCH_CHANNEL);
+			return;
+		}
+	}
+
+	if (channel == MAUSB_ISOCH_CHANNEL) {
+		dev->channel_map[MAUSB_INTR_CHANNEL] =
+				dev->channel_map[MAUSB_CTRL_CHANNEL];
+	}
+}
+
+static void mausb_handle_connect_event(struct mausb_device *dev,
+				       enum mausb_channel channel, int status,
+				       void *data)
+{
+	mausb_connect_callback(dev, channel, status);
+}
+
+static void mausb_handle_receive_event(struct mausb_device *dev,
+				       enum mausb_channel channel, int status,
+				       void *data)
+{
+	struct mausb_event event;
+
+	event.madev_addr = dev->madev_addr;
+
+	if (status <= 0) {
+		mausb_pr_err("Receive event error status=%d", status);
+		queue_work(dev->workq, &dev->socket_disconnect_work);
+		queue_work(dev->workq, &dev->hcd_disconnect_work);
+		return;
+	}
+
+	mausb_reset_connection_timer(dev);
+}
+
+void mausb_ip_callback(void *ctx, enum mausb_channel channel,
+		       enum mausb_link_action action, int status, void *data)
+{
+	struct mausb_device *dev = (struct mausb_device *)ctx;
+
+	switch (action) {
+	case MAUSB_LINK_CONNECT:
+		mausb_handle_connect_event(dev, channel, status, data);
+		break;
+	case MAUSB_LINK_SEND:
+		/*
+		 * Currently there is nothing to do, as send operation is
+		 * synchronous
+		 */
+		break;
+	case MAUSB_LINK_RECV:
+		mausb_handle_receive_event(dev, channel, status, data);
+		break;
+	case MAUSB_LINK_DISCONNECT:
+		/*
+		 * Currently there is nothing to do, as disconnect operation is
+		 * synchronous
+		 */
+		break;
+	default:
+		mausb_pr_warn("Unknown network action");
+	}
+}
+
 static int mausb_read_virtual_buffer(struct mausb_data_iter *iterator,
 				     u32 byte_num,
 				     struct list_head *data_chunks_list,
diff --git a/drivers/usb/mausb_host/hpal.h b/drivers/usb/mausb_host/hpal.h
index f184bbc07783..a04ed120ba5e 100644
--- a/drivers/usb/mausb_host/hpal.h
+++ b/drivers/usb/mausb_host/hpal.h
@@ -131,6 +131,8 @@ static inline u64 mausb_event_id(struct mausb_device *dev)
 	return val;
 }
 
+int mausb_initiate_dev_connection(struct mausb_device_address device_address,
+				  u8 madev_address);
 int mausb_data_req_enqueue_event(struct mausb_device *dev, u16 ep_handle,
 				 struct urb *request);
 int mausb_signal_event(struct mausb_device *dev, struct mausb_event *event,
@@ -158,6 +160,7 @@ static inline void mausb_remove_event(struct mausb_device *dev,
 }
 
 void mausb_release_ma_dev_async(struct kref *kref);
+void mausb_on_madev_connected(struct mausb_device *dev);
 void mausb_complete_request(struct urb *urb, u32 actual_length, int status);
 void mausb_complete_urb(struct mausb_event *event);
 void mausb_reset_connection_timer(struct mausb_device *dev);
@@ -240,6 +243,9 @@ enum mausb_channel mausb_transfer_type_to_channel(u8 transfer_type)
 	return transfer_type >> 3;
 }
 
+void mausb_ip_callback(void *ctx, enum mausb_channel channel,
+		       enum mausb_link_action action, int status, void *data);
+
 struct mausb_data_iter {
 	u32 length;
 
diff --git a/drivers/usb/mausb_host/mausb_core.c b/drivers/usb/mausb_host/mausb_core.c
index 8730590126ea..101afd0b9deb 100644
--- a/drivers/usb/mausb_host/mausb_core.c
+++ b/drivers/usb/mausb_host/mausb_core.c
@@ -13,41 +13,122 @@
 
 #include "hcd.h"
 #include "hpal.h"
+#include "mausb_address.h"
 #include "utils.h"
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("DisplayLink (UK) Ltd.");
 MODULE_VERSION(MAUSB_DRIVER_VERSION);
 
+static struct mausb_device_address	device_address;
+static int				mausb_device_disconnect_param;
+static u16				madev_addr;
+static u8				mausb_client_connect_param;
+static u8				mausb_client_disconnect_param;
+
 static int mausb_client_connect(const char *value,
 				const struct kernel_param *kp)
 {
+	unsigned long flags = 0;
+
 	mausb_pr_info("Version=%s", MAUSB_DRIVER_VERSION);
 
+	spin_lock_irqsave(&mss.lock, flags);
+	if (mss.client_connected) {
+		mausb_pr_err("MA-USB client is already connected");
+		spin_unlock_irqrestore(&mss.lock, flags);
+		return -EEXIST;
+	}
+	/* Save heartbeat client information */
+	mss.client_connected = true;
+	mss.missed_heartbeats = 0;
+	reinit_completion(&mss.client_stopped);
+	spin_unlock_irqrestore(&mss.lock, flags);
+	/* Start hearbeat timer */
+	mod_timer(&mss.heartbeat_timer,
+		  jiffies + msecs_to_jiffies(MAUSB_HEARTBEAT_TIMEOUT_MS));
+
 	return 0;
 }
 
 static int mausb_client_disconnect(const char *value,
 				   const struct kernel_param *kp)
 {
+	unsigned long flags = 0;
+	struct mausb_device *dev = NULL;
+
 	mausb_pr_info("Version=%s", MAUSB_DRIVER_VERSION);
 
+	spin_lock_irqsave(&mss.lock, flags);
+	if (!mss.client_connected) {
+		mausb_pr_err("MA-USB client is not connected");
+		spin_unlock_irqrestore(&mss.lock, flags);
+		return -ENODEV;
+	}
+
+	spin_unlock_irqrestore(&mss.lock, flags);
+
+	/* Stop heartbeat timer */
+	del_timer_sync(&mss.heartbeat_timer);
+
+	/* Clear heartbeat client information */
+	spin_lock_irqsave(&mss.lock, flags);
+	mss.client_connected = false;
+	mss.missed_heartbeats = 0;
+	list_for_each_entry(dev, &mss.madev_list, list_entry) {
+		mausb_pr_debug("Enqueue heartbeat_work madev_addr=%x",
+			       dev->madev_addr);
+		queue_work(dev->workq, &dev->heartbeat_work);
+	}
+	complete(&mss.client_stopped);
+	spin_unlock_irqrestore(&mss.lock, flags);
+
 	return 0;
 }
 
 static int mausb_device_connect(const char *value,
 				const struct kernel_param *kp)
 {
+	int status = 0;
+
 	mausb_pr_info("Version=%s", MAUSB_DRIVER_VERSION);
 
-	return 0;
+	if (strlen(value) <= INET6_ADDRSTRLEN) {
+		strcpy(device_address.ip.address, value);
+		mausb_pr_info("Processing '%s' address",
+			      device_address.ip.address);
+	} else {
+		mausb_pr_err("Invalid IP format");
+		return 0;
+	}
+	status = mausb_initiate_dev_connection(device_address, madev_addr);
+	memset(&device_address, 0, sizeof(device_address));
+
+	return status;
 }
 
 static int mausb_device_disconnect(const char *value,
 				   const struct kernel_param *kp)
 {
+	u8 dev_address = 0;
+	int status = 0;
+	unsigned long flags = 0;
+	struct mausb_device *dev = NULL;
+
 	mausb_pr_info("Version=%s", MAUSB_DRIVER_VERSION);
 
+	status = kstrtou8(value, 0, &dev_address);
+	if (status < 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&mss.lock, flags);
+
+	dev = mausb_get_dev_from_addr_unsafe(dev_address);
+	if (dev)
+		queue_work(dev->workq, &dev->hcd_disconnect_work);
+
+	spin_unlock_irqrestore(&mss.lock, flags);
+
 	return 0;
 }
 
@@ -67,6 +148,26 @@ static const struct kernel_param_ops mausb_client_disconnect_ops = {
 	.set = mausb_client_disconnect
 };
 
+module_param_named(mgmt, device_address.ip.port.management, ushort, 0664);
+MODULE_PARM_DESC(mgmt, "MA-USB management port");
+module_param_named(ctrl, device_address.ip.port.control, ushort, 0664);
+MODULE_PARM_DESC(ctrl, "MA-USB control port");
+module_param_named(bulk, device_address.ip.port.bulk, ushort, 0664);
+MODULE_PARM_DESC(bulk, "MA-USB bulk port");
+module_param_named(isoch, device_address.ip.port.isochronous, ushort, 0664);
+MODULE_PARM_DESC(isoch, "MA-USB isochronous port");
+module_param_named(madev_addr, madev_addr, ushort, 0664);
+MODULE_PARM_DESC(madev_addr, "MA-USB device address");
+
+module_param_cb(client_connect, &mausb_client_connect_ops,
+		&mausb_client_connect_param, 0664);
+module_param_cb(client_disconnect, &mausb_client_disconnect_ops,
+		&mausb_client_disconnect_param, 0664);
+module_param_cb(ip, &mausb_device_connect_ops,
+		device_address.ip.address, 0664);
+module_param_cb(disconnect, &mausb_device_disconnect_ops,
+		&mausb_device_disconnect_param, 0664);
+
 static int mausb_host_init(void)
 {
 	int status;
-- 
2.17.1