From ebf01625908eb8f938a01a0b336d5cf33e17c5d9 Mon Sep 17 00:00:00 2001 From: Bharath Tirunagaru Date: Fri, 13 Mar 2020 16:20:12 -0700 Subject: [PATCH] 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 --- dsp/audio_pdr.c | 33 ++++- dsp/audio_prm.c | 3 +- dsp/voice_mhi.c | 2 +- include/ipc/gpr-lite.h | 8 ++ ipc/audio-pkt.c | 318 +++++++++++++++++++++++++++++++++-------- ipc/gpr-lite.c | 262 ++++++++++++++++++++++++++++++--- 6 files changed, 548 insertions(+), 78 deletions(-) diff --git a/dsp/audio_pdr.c b/dsp/audio_pdr.c index b9341dec1e..4c1fb79333 100644 --- a/dsp/audio_pdr.c +++ b/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); diff --git a/dsp/audio_prm.c b/dsp/audio_prm.c index f82818ac7a..983e8f9a9d 100644 --- a/dsp/audio_prm.c +++ b/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; } diff --git a/dsp/voice_mhi.c b/dsp/voice_mhi.c index 5bc8795ac1..ff45969595 100644 --- a/dsp/voice_mhi.c +++ b/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); diff --git a/include/ipc/gpr-lite.h b/include/ipc/gpr-lite.h index 4e8319bfc3..9c8e6cea2c 100644 --- a/include/ipc/gpr-lite.h +++ b/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 diff --git a/ipc/audio-pkt.c b/ipc/audio-pkt.c index 89ead1090c..d526cd6702 100644 --- a/ipc/audio-pkt.c +++ b/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 - * @adev: gpr device node + * struct audio_pkt_device - driver context, relates to platform dev * @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 device *dev = audpkt_dev->dev; - - AUDIO_PKT_ERR("for %s\n", audpkt_dev->ch_name); - - get_device(dev); - file->private_data = audpkt_dev; - + struct audio_pkt_device *audpkt_dev = ap_priv->ap_dev; + AUDIO_PKT_INFO("%s: for %s \n", __func__,audpkt_dev->ch_name); + file->private_data = ap_priv; 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 device *dev = audpkt_dev->dev; + struct audio_pkt_priv *ap_priv = file->private_data; + 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; - struct device *dev = &adev->dev; - int ret; + if(ap_priv) + { + 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 = { - .probe = audio_pkt_probe, - .remove = audio_pkt_remove, - .callback = audio_pkt_srvc_callback, - .driver = { +static const struct of_device_id audio_pkt_platform_match_table[] = { + { .compatible = "qcom,audio-pkt-core-platform"}, + {} +}; +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"); diff --git a/ipc/gpr-lite.c b/ipc/gpr-lite.c index 4684131612..12e5d81d1d 100644 --- a/ipc/gpr-lite.c +++ b/ipc/gpr-lite.c @@ -22,36 +22,107 @@ #include #include +#include +#include + 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 = devm_kzalloc(dev, sizeof(*gpr), GFP_KERNEL); - if (!gpr) + gpr_priv = devm_kzalloc(dev, sizeof(*gpr_priv), GFP_KERNEL); + if (!gpr_priv) 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); - gpr->ch = rpdev->ept; - gpr->dev = dev; - spin_lock_init(&gpr->svcs_lock); - idr_init(&gpr->svcs_idr); + dev_info(dev, "%s: registered via subsys_notif_register for domain id(%d)", + __func__, gpr_priv->dest_domain_id ); + + dev_set_drvdata(dev, gpr_priv); + 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); }