audio-kernel: Add changes to support SSR/PDR

Add changes in audio-pkt,voice_mhi and gpr-lite drivers.
Fixing bug in audio_pdr driver w.r.t memory allocation.

Change-Id: I6ab7ff13b532921f85e9548341969abd32ee3b6d

Signed-off-by: Bharath Tirunagaru <bharatht@codeaurora.org>

dsp/audio_pdr.c

@@ -33,11 +33,34 @@ static int audio_pdr_locator_callback(struct notifier_block *this,
 	memcpy(&audio_pdr_services, data,
 		sizeof(audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP]));
 	if (audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].total_domains == 1) {
+		int domain_list_size = (sizeof(struct servreg_loc_entry_v01)) *
+			            (audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].total_domains);
+		struct servreg_loc_entry_v01 *domain_list_temp = NULL;
+
 		pr_debug("%s: Service %s, returned total domains %d, ",
 			__func__,
 			audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].service_name,
 			audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].
 			total_domains);
+
+		domain_list_temp = (struct servreg_loc_entry_v01 *)kzalloc(
+			sizeof(domain_list_size), GFP_KERNEL);
+
+		if(!domain_list_size)
+		{
+			pr_err("%s: Service %s total domains %d could not allocate memory",
+			__func__,
+			audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].service_name,
+			audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].
+			total_domains);
+			goto done;
+		} else {
+			memcpy(domain_list_temp,
+				audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].domain_list,
+				domain_list_size);
+			audio_pdr_services[AUDIO_PDR_DOMAIN_ADSP].domain_list = domain_list_temp;
+		}
+
 		pdr_state = AUDIO_PDR_FRAMEWORK_UP;
 		goto done;
 	} else
@@ -108,7 +131,7 @@ void *audio_pdr_service_register(int domain_id,
 		audio_pdr_services[domain_id].domain_list[0].instance_id,
 		nb, curr_state);
 	if (IS_ERR_OR_NULL(handle)) {
-		pr_err("%s: Failed to register for service %s, instance %d\n",
+		pr_err("%s: Failed to register for domain %s, instance %d\n",
 			__func__,
 			audio_pdr_services[domain_id].domain_list[0].name,
 			audio_pdr_services[domain_id].domain_list[0].
@@ -168,6 +191,14 @@ module_init(audio_pdr_late_init);
 
 static void __exit audio_pdr_late_exit(void)
 {
+	int i;
+
+	for (i = 0; i < AUDIO_PDR_DOMAIN_MAX; i++)
+	{
+		if (audio_pdr_services[i].domain_list != NULL)
+			kfree(audio_pdr_services[i].domain_list);
+	}
+
 }
 module_exit(audio_pdr_late_exit);
 

dsp/audio_prm.c

@@ -357,9 +357,8 @@ static int audio_prm_remove(struct gpr_device *adev)
 		goto err;
 	}
 	g_prm.adev = NULL;
+	g_prm.is_adsp_up = false;
 	mutex_unlock(&g_prm.lock);
-
-	kfree(&g_prm);
 err:
 	return ret;
 }

dsp/voice_mhi.c

@@ -488,8 +488,8 @@ static int voice_mhi_gpr_probe(struct gpr_device *gdev)
 
 static int voice_mhi_gpr_exit(struct gpr_device *gdev)
 {
-	mutex_lock(&voice_mhi_lcl.mutex);
 	voice_mhi_end();
+	mutex_lock(&voice_mhi_lcl.mutex);
 	voice_mhi_lcl.gdev = NULL;
 	VOICE_MHI_STATE_RESET(voice_mhi_lcl.voice_mhi_state,
 					VOICE_MHI_ADSP_UP);

include/ipc/gpr-lite.h

@@ -29,6 +29,14 @@ enum gpr_subsys_state {
 };
 
 
+struct gpr_q6 {
+ //void *pil;
+ atomic_t q6_state;
+ atomic_t modem_state;
+ struct mutex lock;
+};
+
+
 /* Version */
 #define GPR_PKT_VER		0x0
 

ipc/audio-pkt.c

@@ -62,12 +62,19 @@ do {									      \
 #define AUDPKT_DRIVER_NAME "aud_pasthru_adsp"
 #define CHANNEL_NAME "to_apps"
 
+
+enum audio_pkt_state {
+	AUDIO_PKT_INIT,
+	AUDIO_PKT_PROBED,
+	AUDIO_PKT_REMOVED,
+	AUDIO_PKT_DEINIT,
+};
+
 /**
- * struct audio_pkt - driver context, relates rpdev to cdev
+ * struct audio_pkt_device - driver context, relates to platform dev
- * @adev:	gpr device node
  * @dev:	audio pkt device
  * @cdev:	cdev for the audio pkt device
- * @lock:	synchronization of @rpdev
+ * @lock:	synchronization of @dev
  * @queue_lock:	synchronization of @queue operations
  * @queue:	incoming message queue
  * @readq:	wait object for incoming queue
@@ -77,12 +84,10 @@ do {									      \
  * @audio_pkt_class: audio pkt class pointer
  */
 struct audio_pkt_device {
-	struct gpr_device *adev;
 	struct device *dev;
 	struct cdev cdev;
 
 	struct mutex lock;
-
 	spinlock_t queue_lock;
 	struct sk_buff_head queue;
 	wait_queue_head_t readq;
@@ -94,6 +99,18 @@ struct audio_pkt_device {
 	struct class *audio_pkt_class;
 };
 
+struct audio_pkt_priv {
+	struct gpr_device *adev;
+	struct device *dev;
+	struct audio_pkt_device *ap_dev;
+
+	struct mutex lock;
+	enum audio_pkt_state status;
+};
+
+static struct audio_pkt_priv *ap_priv;
+
+
 struct audio_pkt_apm_cmd_shared_mem_map_regions_t {
 	uint16_t mem_pool_id;
 	uint16_t num_regions;
@@ -124,9 +141,6 @@ struct audio_pkt_clnt_ch {
 	audio_pkt_clnt_cb_fn func;
 };
 
-#define dev_to_audpkt_dev(_dev) container_of(_dev, struct audio_pkt_device, dev)
-#define cdev_to_audpkt_dev(_cdev) container_of(_cdev, struct audio_pkt_device, cdev)
-
 /**
  * audio_pkt_open() - open() syscall for the audio_pkt device
  * inode:	Pointer to the inode structure.
@@ -138,14 +152,9 @@ struct audio_pkt_clnt_ch {
  */
 int audio_pkt_open(struct inode *inode, struct file *file)
 {
-	struct audio_pkt_device *audpkt_dev = cdev_to_audpkt_dev(inode->i_cdev);
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
-	struct device *dev = audpkt_dev->dev;
+	AUDIO_PKT_INFO("%s: for %s \n", __func__,audpkt_dev->ch_name);
-
+	file->private_data = ap_priv;
-	AUDIO_PKT_ERR("for %s\n", audpkt_dev->ch_name);
-
-	get_device(dev);
-	file->private_data = audpkt_dev;
-
 	return 0;
 }
 
@@ -160,12 +169,18 @@ int audio_pkt_open(struct inode *inode, struct file *file)
  */
 int audio_pkt_release(struct inode *inode, struct file *file)
 {
-	struct audio_pkt_device *audpkt_dev = cdev_to_audpkt_dev(inode->i_cdev);
+	struct audio_pkt_priv *ap_priv = file->private_data;
-	struct device *dev = audpkt_dev->dev;
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
+
 	struct sk_buff *skb;
 	unsigned long flags;
 
-	AUDIO_PKT_INFO("for %s \n", audpkt_dev->ch_name);
+	if ((!audpkt_dev)) {
+		AUDIO_PKT_ERR("invalid device handle\n");
+		return -EINVAL;
+	}
+
+	AUDIO_PKT_INFO("%s: for %s \n", __func__,audpkt_dev->ch_name);
 	spin_lock_irqsave(&audpkt_dev->queue_lock, flags);
 
 	/* Discard all SKBs */
@@ -176,12 +191,38 @@ int audio_pkt_release(struct inode *inode, struct file *file)
 	wake_up_interruptible(&audpkt_dev->readq);
 	spin_unlock_irqrestore(&audpkt_dev->queue_lock, flags);
 
-	put_device(dev);
 	file->private_data = NULL;
 
 	return 0;
 }
 
+
+static int audio_pkt_internal_release(struct platform_device *adev)
+{
+	struct audio_pkt_priv *ap_priv = platform_get_drvdata(adev);
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
+	struct sk_buff *skb;
+	unsigned long flags;
+
+	if ((!audpkt_dev)) {
+		AUDIO_PKT_ERR("invalid device handle\n");
+		return -EINVAL;
+	}
+
+	AUDIO_PKT_INFO("%s: for %s\n", __func__,audpkt_dev->ch_name);
+	spin_lock_irqsave(&audpkt_dev->queue_lock, flags);
+	/* Discard all SKBs */
+	while (!skb_queue_empty(&audpkt_dev->queue)) {
+		skb = skb_dequeue(&audpkt_dev->queue);
+		kfree_skb(skb);
+	}
+	spin_unlock_irqrestore(&audpkt_dev->queue_lock, flags);
+
+	wake_up_interruptible(&audpkt_dev->readq);
+
+	return 0;
+}
+
 /**
  * audio_pkt_read() - read() syscall for the audio_pkt device
  * file:	Pointer to the file structure.
@@ -196,7 +237,9 @@ int audio_pkt_release(struct inode *inode, struct file *file)
 ssize_t audio_pkt_read(struct file *file, char __user *buf,
 		       size_t count, loff_t *ppos)
 {
-	struct audio_pkt_device *audpkt_dev = file->private_data;
+	struct audio_pkt_priv *ap_priv = file->private_data;
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
+
 	unsigned long flags;
 	struct sk_buff *skb;
 	int use;
@@ -207,6 +250,15 @@ ssize_t audio_pkt_read(struct file *file, char __user *buf,
 		return -EINVAL;
 	}
 
+	mutex_lock(&ap_priv->lock);
+	if (AUDIO_PKT_PROBED != ap_priv->status)
+	{
+		mutex_unlock(&ap_priv->lock);
+		AUDIO_PKT_ERR("dev is in reset\n");
+		return -ENETRESET;
+	}
+	mutex_unlock(&ap_priv->lock);
+
 	spin_lock_irqsave(&audpkt_dev->queue_lock, flags);
 	/* Wait for data in the queue */
 	if (skb_queue_empty(&audpkt_dev->queue)) {
@@ -278,7 +330,8 @@ int audpkt_chk_and_update_physical_addr(struct audio_gpr_pkt *gpr_pkt)
 ssize_t audio_pkt_write(struct file *file, const char __user *buf,
 			size_t count, loff_t *ppos)
 {
-	struct audio_pkt_device *audpkt_dev = file->private_data;
+	struct audio_pkt_priv *ap_priv = file->private_data;
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
 	struct gpr_hdr *audpkt_hdr = NULL;
 	void *kbuf;
 	int ret;
@@ -288,6 +341,15 @@ ssize_t audio_pkt_write(struct file *file, const char __user *buf,
 		return -EINVAL;
 	}
 
+	mutex_lock(&ap_priv->lock);
+	if (AUDIO_PKT_PROBED != ap_priv->status)
+	{
+		mutex_unlock(&ap_priv->lock);
+		AUDIO_PKT_ERR("dev is in reset\n");
+		return -ENETRESET;
+	}
+	mutex_unlock(&ap_priv->lock);
+
 	kbuf = memdup_user(buf, count);
 	if (IS_ERR(kbuf))
 		return PTR_ERR(kbuf);
@@ -305,9 +367,10 @@ ssize_t audio_pkt_write(struct file *file, const char __user *buf,
 		ret = -ERESTARTSYS;
 		goto free_kbuf;
 	}
-	ret = gpr_send_pkt(audpkt_dev->adev,(struct gpr_pkt *) kbuf);
+	ret = gpr_send_pkt(ap_priv->adev,(struct gpr_pkt *) kbuf);
 	if (ret < 0) {
 		AUDIO_PKT_ERR("APR Send Packet Failed ret -%d\n", ret);
+		mutex_unlock(&audpkt_dev->lock);
 		return ret;
 	}
 	mutex_unlock(&audpkt_dev->lock);
@@ -328,18 +391,16 @@ free_kbuf:
  */
 static unsigned int audio_pkt_poll(struct file *file, poll_table *wait)
 {
-	struct audio_pkt_device *audpkt_dev = file->private_data;
+	struct audio_pkt_priv *ap_priv = file->private_data;
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
 	unsigned int mask = 0;
 	unsigned long flags;
-
-	audpkt_dev = file->private_data;
 	if (!audpkt_dev) {
 		AUDIO_PKT_ERR("invalid device handle\n");
 		return POLLERR;
 	}
 
 	poll_wait(file, &audpkt_dev->readq, wait);
-
 	mutex_lock(&audpkt_dev->lock);
 
 	spin_lock_irqsave(&audpkt_dev->queue_lock, flags);
@@ -347,7 +408,6 @@ static unsigned int audio_pkt_poll(struct file *file, poll_table *wait)
 		mask |= POLLIN | POLLRDNORM;
 
 	spin_unlock_irqrestore(&audpkt_dev->queue_lock, flags);
-
 	mutex_unlock(&audpkt_dev->lock);
 
 	return mask;
@@ -372,7 +432,9 @@ static const struct file_operations audio_pkt_fops = {
 static int audio_pkt_srvc_callback(struct gpr_device *adev,
 				void *data)
 {
-	struct audio_pkt_device *audpkt_dev = dev_get_drvdata(&adev->dev);
+	struct audio_pkt_priv *ap_priv =  dev_get_drvdata(&adev->dev);
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
+
 	unsigned long flags;
 	struct sk_buff *skb;
 	struct gpr_hdr *hdr = (struct gpr_hdr *)data;
@@ -380,7 +442,8 @@ static int audio_pkt_srvc_callback(struct gpr_device *adev,
 	hdr_size = GPR_PKT_GET_HEADER_BYTE_SIZE(hdr->header);
 	pkt_size = GPR_PKT_GET_PACKET_BYTE_SIZE(hdr->header);
 
-        AUDIO_PKT_INFO("header %d packet %d \n",hdr_size, pkt_size);
+    AUDIO_PKT_INFO("%s: header %d packet %d \n",
+		__func__,hdr_size, pkt_size);
 
 	skb = alloc_skb(pkt_size, GFP_ATOMIC);
 	if (!skb)
@@ -409,11 +472,117 @@ static int audio_pkt_srvc_callback(struct gpr_device *adev,
  */
 static int audio_pkt_probe(struct gpr_device *adev)
 {
-	struct audio_pkt_device *audpkt_dev;
+	if(ap_priv)
-	struct device *dev = &adev->dev;
+	{
-	int ret;
+		mutex_lock(&ap_priv->lock);
+		ap_priv->adev = adev;
+		ap_priv->status = AUDIO_PKT_PROBED;
+		mutex_unlock(&ap_priv->lock);
 
-	audpkt_dev = devm_kzalloc(dev, sizeof(*audpkt_dev), GFP_KERNEL);
+		dev_set_drvdata(&adev->dev, ap_priv);
+
+		dev_dbg(&adev->dev, "%s: Driver[%s] Probed\n",
+		 __func__, adev->name);
+	}
+	else
+	{
+		dev_err(&adev->dev, "%s: Driver[%s] Probe Failed\n",
+		 __func__, adev->name);
+		return -EINVAL;
+	}
+
+	return 0;
+
+}
+
+/**
+ * audio_pkt_remove() - Remove a AUDIO packet device
+ *
+ * adev:	Pointer to gpr device.
+ *
+ * return:	0 on success, standard Linux error codes on error.
+ *
+ * This function is called when the underlying device tree driver
+ * removeds a gpr device, mapped to a Audio packet device.
+ */
+static int audio_pkt_remove(struct gpr_device *adev)
+{
+	if(ap_priv)
+	{
+		mutex_lock(&ap_priv->lock);
+		ap_priv->adev = NULL;
+		ap_priv->status = AUDIO_PKT_REMOVED;
+		mutex_unlock(&ap_priv->lock);
+		dev_dbg(&adev->dev, "%s: Driver[%s] Removing\n",
+		 __func__, adev->name);
+		dev_set_drvdata(&adev->dev, NULL);
+	}
+	else
+	{
+		dev_err(&adev->dev, "%s: Driver[%s] Remove Failed\n",
+		 __func__, adev->name);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static const struct of_device_id audio_pkt_match_table[] = {
+	{ .compatible = "qcom,audio-pkt" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, audio_pkt_match_table);
+
+static struct gpr_driver audio_pkt_driver = {
+	.probe = audio_pkt_probe,
+	.remove = audio_pkt_remove,
+	.callback = audio_pkt_srvc_callback,
+	.driver = {
+		.name = MODULE_NAME,
+		.of_match_table = of_match_ptr(audio_pkt_match_table),
+	 },
+};
+
+static int audio_pkt_plaform_driver_register_gpr(struct platform_device *pdev,
+				struct audio_pkt_device *audpkt_dev)
+{
+	int ret = 0;
+
+	ap_priv = devm_kzalloc(&pdev->dev,
+			     sizeof(*ap_priv), GFP_KERNEL);
+	if (!ap_priv)
+		return -ENOMEM;
+
+	ret = gpr_driver_register(&audio_pkt_driver);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "%s: registering to gpr driver failed, err = %d\n",
+			__func__, ret);
+		goto err;
+	}
+
+	mutex_init(&ap_priv->lock);
+	ap_priv->status = AUDIO_PKT_INIT;
+	ap_priv->ap_dev = audpkt_dev;
+	ap_priv->dev = audpkt_dev->dev;
+err:
+	return ret;
+}
+
+/**
+ * audio_pkt_platform_driver_probe() - Probe a AUDIO packet device
+ *
+ * adev:	Pointer to platform device.
+ *
+ * return:	0 on success, standard Linux error codes on error.
+ *
+ * This function is called when the underlying device tree driver registers
+ * a platform device, mapped to a Audio packet device.
+ */
+static int audio_pkt_platform_driver_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct audio_pkt_device *audpkt_dev;
+
+	audpkt_dev = devm_kzalloc(&pdev->dev, sizeof(*audpkt_dev), GFP_KERNEL);
 	if (!audpkt_dev)
 		return -ENOMEM;
 
@@ -452,9 +621,6 @@ static int audio_pkt_probe(struct gpr_device *adev)
 	skb_queue_head_init(&audpkt_dev->queue);
 	init_waitqueue_head(&audpkt_dev->readq);
 
-	audpkt_dev->adev = adev;
-	dev_set_drvdata(dev, audpkt_dev);
-
 	cdev_init(&audpkt_dev->cdev, &audio_pkt_fops);
 	audpkt_dev->cdev.owner = THIS_MODULE;
 
@@ -466,12 +632,20 @@ static int audio_pkt_probe(struct gpr_device *adev)
 		goto free_dev;
 	}
 
+	ret = audio_pkt_plaform_driver_register_gpr(pdev, audpkt_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "%s: Failed to register with gpr, err = %d\n",
+			__func__, ret);
+		goto free_dev;
+	}
+
+	platform_set_drvdata(pdev, ap_priv);
 	AUDIO_PKT_INFO("Audio Packet Port Driver Initialized\n");
 
-	return of_platform_populate(dev->of_node, NULL, NULL, dev);
+	goto done;
+	//return of_platform_populate(dev->of_node, NULL, NULL, dev);
 
 free_dev:
-	put_device(dev);
 	device_destroy(audpkt_dev->audio_pkt_class,audpkt_dev->audio_pkt_major);
 err_device:
 	class_destroy(audpkt_dev->audio_pkt_class);
@@ -479,45 +653,73 @@ err_class:
 	unregister_chrdev_region(MAJOR(audpkt_dev->audio_pkt_major),
 				 MINOR_NUMBER_COUNT);
 err_chrdev:
-	kfree(audpkt_dev);
+done:
 	return ret;
 
 }
 
 /**
- * audio_pkt_remove() - Remove a AUDIO packet device
+ * audio_pkt_platform_driver_remove() - Remove a AUDIO packet platform device
  *
- * adev:	Pointer to gpr device.
+ * adev:	Pointer to platform device.
  *
  * return:	0 on success, standard Linux error codes on error.
  *
  * This function is called when the underlying device tree driver
- * removeds a gpr device, mapped to a Audio packet device.
+ * removes a platform device, mapped to a Audio packet device.
  */
-static int audio_pkt_remove(struct gpr_device *adev)
+static int audio_pkt_platform_driver_remove(struct platform_device *adev)
 {
-	of_platform_depopulate(&adev->dev);
+	struct audio_pkt_priv *ap_priv = platform_get_drvdata(adev);
+	struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev;
+
+	gpr_driver_unregister(&audio_pkt_driver);
+
+	audio_pkt_internal_release(adev);
+
+	if (audpkt_dev) {
+		cdev_del(&audpkt_dev->cdev);
+		device_destroy(audpkt_dev->audio_pkt_class,audpkt_dev->audio_pkt_major);
+		class_destroy(audpkt_dev->audio_pkt_class);
+		unregister_chrdev_region(MAJOR(audpkt_dev->audio_pkt_major),
+				 MINOR_NUMBER_COUNT);
+	}
+
+	//of_platform_depopulate(&adev->dev);
 	AUDIO_PKT_INFO("Audio Packet Port Driver Removed\n");
 
 	return 0;
 
 }
-static const struct of_device_id audio_pkt_match_table[] = {
-	{ .compatible = "qcom,audio-pkt" },
-	{},
-};
-MODULE_DEVICE_TABLE(of, audio_pkt_match_table);
 
-static struct gpr_driver audio_pkt_driver = {
+static const struct of_device_id audio_pkt_platform_match_table[] = {
-	.probe = audio_pkt_probe,
+	{ .compatible = "qcom,audio-pkt-core-platform"},
-	.remove = audio_pkt_remove,
+	{}
-	.callback = audio_pkt_srvc_callback,
+};
-	.driver = {
+MODULE_DEVICE_TABLE(of, audio_pkt_platform_match_table);
+
+
+static struct platform_driver audio_pkt_core_platform_driver = {
+	.probe          = audio_pkt_platform_driver_probe,
+	.remove         = audio_pkt_platform_driver_remove,
+	.driver         = {
 		.name = MODULE_NAME,
-		.of_match_table = of_match_ptr(audio_pkt_match_table),
+		.of_match_table = of_match_ptr(audio_pkt_platform_match_table),
-	 },
+	},
 };
 
-module_gpr_driver(audio_pkt_driver);
+
+static int __init audio_pkt_init(void)
+{
+	return platform_driver_register(&audio_pkt_core_platform_driver);
+}
+
+static void __exit audio_pkt_exit(void)
+{
+	platform_driver_unregister(&audio_pkt_core_platform_driver);
+}
+module_init(audio_pkt_init);
+module_exit(audio_pkt_exit);
+
 MODULE_DESCRIPTION("MSM Audio Packet Driver");
 MODULE_LICENSE("GPL v2");

ipc/gpr-lite.c

@@ -22,36 +22,107 @@
 #include <linux/rpmsg.h>
 #include <linux/of.h>
 
+#include <soc/snd_event.h>
+#include <dsp/audio_notifier.h>
+
 struct gpr {
 	struct rpmsg_endpoint *ch;
 	struct device *dev;
+	spinlock_t gpr_lock;
+	bool is_initial_boot;
+
 	spinlock_t svcs_lock;
 	struct idr svcs_idr;
 	int dest_domain_id;
 };
 
+static struct gpr_q6 q6;
+static struct gpr *gpr_priv;
+
+enum gpr_subsys_state gpr_get_q6_state(void)
+{
+	return atomic_read(&q6.q6_state);
+}
+EXPORT_SYMBOL(gpr_get_q6_state);
+
+enum gpr_subsys_state gpr_get_modem_state(void)
+{
+	return atomic_read(&q6.modem_state);
+}
+EXPORT_SYMBOL(gpr_get_modem_state);
+
+
+void gpr_subsys_notif_register(char *client_name, int domain,
+			   struct notifier_block *nb)
+{
+	int ret;
+
+	ret = audio_notifier_register(client_name, domain, nb);
+	if (ret < 0)
+		dev_err(gpr_priv->dev, "%s: Audio notifier register failed for domain %d ret = %d\n",
+			__func__, domain, ret);
+}
+
+void gpr_subsys_notif_deregister(char *client_name)
+{
+	int ret;
+
+	ret = audio_notifier_deregister(client_name);
+	if (ret < 0)
+		dev_err(gpr_priv->dev, "%s: Audio notifier de-register failed for client %s\n",
+			__func__, client_name);
+}
+
 /**
  * gpr_send_pkt() - Send a gpr message from gpr device
  *
  * @adev: Pointer to previously registered gpr device.
  * @pkt: Pointer to gpr packet to send
  *
- * Return: Will be an negative on packet size on success.
+ * Return: Will be an negative and/or packet size on success.
  */
 int gpr_send_pkt(struct gpr_device *adev, struct gpr_pkt *pkt)
 {
-	struct gpr *gpr = dev_get_drvdata(adev->dev.parent);
+	struct gpr *gpr;
 	struct gpr_hdr *hdr;
 	unsigned long flags;
 	uint32_t pkt_size;
 	int ret;
 
+	if(!adev)
+	{
+		pr_err("%s: enter pointer adev[%p] \n", __func__, adev);
+		return -EINVAL;
+	}
+
+	if(!(adev->dev.parent))
+	{
+		pr_err("%s: enter pointer adev->dev.parent[%p] \n",
+			__func__, adev->dev.parent);
+		return -EINVAL;
+	}
+
+	gpr = dev_get_drvdata(adev->dev.parent);
+
+	if ((adev->domain_id == GPR_IDS_DOMAIN_ID_ADSP_V) &&
+	    (gpr_get_q6_state() != GPR_SUBSYS_LOADED)) {
+		dev_err(gpr->dev,"%s: domain_id[%d], Still Dsp is not Up\n",
+			__func__, adev->domain_id);
+		return -ENETRESET;
+		} else if ((adev->domain_id == GPR_IDS_DOMAIN_ID_MODEM_V) &&
+		   (gpr_get_modem_state() == GPR_SUBSYS_DOWN)) {
+		dev_err(gpr->dev, "%s: domain_id[%d], Still Modem is not Up\n",
+			__func__, adev->domain_id );
+		return -ENETRESET;
+	}
+
 	spin_lock_irqsave(&adev->lock, flags);
 
 	hdr = &pkt->hdr;
 	pkt_size = GPR_PKT_GET_PACKET_BYTE_SIZE(hdr->header);
 
-	dev_dbg(gpr->dev, "SVC_ID %d %s packet size %d\n", adev->svc_id, __func__, pkt_size);
+	dev_dbg(gpr->dev, "SVC_ID %d %s packet size %d\n",
+		adev->svc_id, __func__, pkt_size);
 	ret = rpmsg_trysend(gpr->ch, pkt, pkt_size);
 	spin_unlock_irqrestore(&adev->lock, flags);
 
@@ -59,6 +130,126 @@ int gpr_send_pkt(struct gpr_device *adev, struct gpr_pkt *pkt)
 }
 EXPORT_SYMBOL_GPL(gpr_send_pkt);
 
+ /**
+  * apr_set_modem_state - Update modem load status.
+  *
+  * @state: State to update modem load status
+  *
+ */
+void gpr_set_modem_state(enum gpr_subsys_state state)
+{
+	atomic_set(&q6.modem_state, state);
+}
+EXPORT_SYMBOL(gpr_set_modem_state);
+
+
+static void gpr_modem_down(unsigned long opcode)
+{
+	gpr_set_modem_state(GPR_SUBSYS_DOWN);
+	//dispatch_event(opcode, APR_DEST_MODEM);
+}
+
+static void gpr_modem_up(void)
+{
+	//if (apr_cmpxchg_modem_state(APR_SUBSYS_DOWN, APR_SUBSYS_UP) ==
+	//						APR_SUBSYS_DOWN)
+	//	wake_up(&modem_wait);
+	//is_modem_up = 1;
+}
+
+
+int gpr_set_q6_state(enum gpr_subsys_state state)
+{
+	dev_dbg(gpr_priv->dev,"%s: setting adsp state %d\n", __func__, state);
+	if (state < GPR_SUBSYS_DOWN || state > GPR_SUBSYS_LOADED)
+		return -EINVAL;
+	atomic_set(&q6.q6_state, state);
+	return 0;
+}
+EXPORT_SYMBOL(gpr_set_q6_state);
+
+static void gpr_ssr_disable(struct device *dev, void *data)
+{
+	gpr_set_q6_state(GPR_SUBSYS_DOWN);
+}
+
+static const struct snd_event_ops gpr_ssr_ops = {
+	.disable = gpr_ssr_disable,
+};
+
+static void gpr_adsp_down(unsigned long opcode)
+{
+	dev_dbg(gpr_priv->dev,"%s: Q6 is Down\n", __func__);
+	snd_event_notify(gpr_priv->dev, SND_EVENT_DOWN);
+	gpr_set_q6_state(GPR_SUBSYS_DOWN);
+}
+
+static void gpr_adsp_up(void)
+{
+	dev_dbg(gpr_priv->dev,"%s: Q6 is Up\n", __func__);
+	gpr_set_q6_state(GPR_SUBSYS_LOADED);
+	snd_event_notify(gpr_priv->dev, SND_EVENT_UP);
+}
+
+static int gpr_notifier_service_cb(struct notifier_block *this,
+			   unsigned long opcode, void *data)
+{
+	struct audio_notifier_cb_data *cb_data = data;
+
+	if (cb_data == NULL) {
+		dev_err(gpr_priv->dev,"%s: Callback data is NULL!\n", __func__);
+		goto done;
+	}
+
+	dev_dbg(gpr_priv->dev,"%s: Service opcode 0x%lx, domain %d\n",
+		__func__, opcode, cb_data->domain);
+
+	switch (opcode) {
+	case AUDIO_NOTIFIER_SERVICE_DOWN:
+		/*
+		 * Use flag to ignore down notifications during
+		 * initial boot. There is no benefit from error
+		 * recovery notifications during initial boot
+		 * up since everything is expected to be down.
+		 */
+		spin_lock(&gpr_priv->gpr_lock);
+		if (gpr_priv->is_initial_boot) {
+			spin_unlock(&gpr_priv->gpr_lock);
+			break;
+		}
+		spin_unlock(&gpr_priv->gpr_lock);
+		if (cb_data->domain == AUDIO_NOTIFIER_MODEM_DOMAIN)
+			gpr_modem_down(opcode);
+		else
+			gpr_adsp_down(opcode);
+		break;
+	case AUDIO_NOTIFIER_SERVICE_UP:
+		if (cb_data->domain == AUDIO_NOTIFIER_MODEM_DOMAIN)
+			gpr_modem_up();
+		else
+			gpr_adsp_up();
+		spin_lock(&gpr_priv->gpr_lock);
+		gpr_priv->is_initial_boot = false;
+		spin_unlock(&gpr_priv->gpr_lock);
+		break;
+	default:
+		break;
+	}
+done:
+	return NOTIFY_OK;
+}
+
+static struct notifier_block adsp_service_nb = {
+	.notifier_call  = gpr_notifier_service_cb,
+	.priority = 0,
+};
+
+static struct notifier_block modem_service_nb = {
+	.notifier_call  = gpr_notifier_service_cb,
+	.priority = 0,
+};
+
+
 static void gpr_dev_release(struct device *dev)
 {
 	struct gpr_device *adev = to_gpr_device(dev);
@@ -100,7 +291,8 @@ static int gpr_callback(struct rpmsg_device *rpdev, void *buf,
 		return -EINVAL;
 	}
 
-	dev_dbg(gpr->dev, "%s: dst_port %x hdr_size %d pkt_size %d\n",__func__ , hdr->dst_port, hdr_size, pkt_size);
+	dev_dbg(gpr->dev, "%s: dst_port %x hdr_size %d pkt_size %d\n",
+	__func__ , hdr->dst_port, hdr_size, pkt_size);
 
 	svc_id = hdr->dst_port;
 	spin_lock_irqsave(&gpr->svcs_lock, flags);
@@ -267,40 +459,78 @@ static void of_register_gpr_devices(struct device *dev)
 static int gpr_probe(struct rpmsg_device *rpdev)
 {
 	struct device *dev = &rpdev->dev;
-	struct gpr *gpr;
 	int ret;
-
+	gpr_priv = devm_kzalloc(dev, sizeof(*gpr_priv), GFP_KERNEL);
-	gpr = devm_kzalloc(dev, sizeof(*gpr), GFP_KERNEL);
+	if (!gpr_priv)
-	if (!gpr)
 		return -ENOMEM;
 
-	ret = of_property_read_u32(dev->of_node, "reg", &gpr->dest_domain_id);
+	spin_lock_init(&gpr_priv->gpr_lock);
+	mutex_init(&q6.lock);
+
+	spin_lock(&gpr_priv->gpr_lock);
+	gpr_priv->is_initial_boot = true;
+	spin_unlock(&gpr_priv->gpr_lock);
+
+	ret = of_property_read_u32(dev->of_node, "reg", &gpr_priv->dest_domain_id);
 	if (ret) {
 		dev_err(dev, "GPR Domain ID not specified in DT\n");
 		return ret;
 	}
+	if (GPR_DOMAIN_ADSP == gpr_priv->dest_domain_id) {
+		gpr_subsys_notif_register("gpr_adsp",
+				       AUDIO_NOTIFIER_ADSP_DOMAIN,
+				       &adsp_service_nb);
+	} else if (GPR_DOMAIN_MODEM == gpr_priv->dest_domain_id) {
+		gpr_subsys_notif_register("gpr_modem",
+				       AUDIO_NOTIFIER_MODEM_DOMAIN,
+				       &modem_service_nb);
+	} else {
+		dev_err(dev, "%s: invalid dest_domain_id %s\n", __func__,
+		  gpr_priv->dest_domain_id);
+		return -EINVAL;
+	}
 
-	dev_set_drvdata(dev, gpr);
+	dev_info(dev, "%s: registered via subsys_notif_register for domain id(%d)",
-	gpr->ch = rpdev->ept;
+		__func__, gpr_priv->dest_domain_id  );
-	gpr->dev = dev;
+
-	spin_lock_init(&gpr->svcs_lock);
+	dev_set_drvdata(dev, gpr_priv);
-	idr_init(&gpr->svcs_idr);
+	gpr_priv->ch = rpdev->ept;
+	gpr_priv->dev = dev;
+	spin_lock_init(&gpr_priv->svcs_lock);
+	idr_init(&gpr_priv->svcs_idr);
 	of_register_gpr_devices(dev);
 
+	ret = snd_event_client_register(&rpdev->dev, &gpr_ssr_ops, NULL);
+	if (ret) {
+		dev_err(dev,"%s: Registration with SND event fwk failed ret = %d\n",
+			__func__, ret);
+		ret = 0;
+	}
+
 	return 0;
 }
 
 static int gpr_remove_device(struct device *dev, void *null)
 {
 	struct gpr_device *adev = to_gpr_device(dev);
-
 	device_unregister(&adev->dev);
-
 	return 0;
 }
 
 static void gpr_remove(struct rpmsg_device *rpdev)
 {
+	struct device *dev = &rpdev->dev;
+	snd_event_client_deregister(&rpdev->dev);
+	dev_info(dev, "%s: deregistering via subsys_notif_register for domain_id(%d)",
+		__func__, gpr_priv->dest_domain_id );
+	if (GPR_DOMAIN_ADSP == gpr_priv->dest_domain_id)
+	{
+		gpr_subsys_notif_deregister("gpr_adsp");
+	}
+	else if(GPR_DOMAIN_MODEM == gpr_priv->dest_domain_id)
+	{
+		gpr_subsys_notif_deregister("gpr_modem");
+	}
 	device_for_each_child(&rpdev->dev, NULL, gpr_remove_device);
 }