diff --git a/audioreach-driver/q6apm_audio_pkt.c b/audioreach-driver/q6apm_audio_pkt.c index ce5097e..84f9efb 100644 --- a/audioreach-driver/q6apm_audio_pkt.c +++ b/audioreach-driver/q6apm_audio_pkt.c @@ -55,7 +55,6 @@ do { \ #define MODULE_NAME "audio-pkt" #define MINOR_NUMBER_COUNT 1 #define AUDPKT_DRIVER_NAME "aud_pasthru_adsp" -#define APM_AUDIO_DRV_NAME "q6apm-audio-pkt" struct q6apm_audio_pkt { struct device *dev; @@ -65,10 +64,8 @@ struct q6apm_audio_pkt { //wait_queue_head_t wait; struct gpr_ibasic_rsp_result_t result; - struct mutex cmd_lock; uint32_t state; - struct cdev cdev; struct mutex lock; spinlock_t queue_lock; @@ -320,7 +317,6 @@ static ssize_t audio_pkt_read(struct file *file, char __user *buf, unsigned long flags; struct sk_buff *skb; int use; - uint32_t *temp; if (!audpkt_dev) { AUDIO_PKT_ERR("invalid device handle\n"); @@ -351,7 +347,6 @@ static ssize_t audio_pkt_read(struct file *file, char __user *buf, use = min_t(size_t, count, skb->len); if (copy_to_user(buf, skb->data, use)) use = -EFAULT; - temp = (uint32_t *) skb->data; kfree_skb(skb); return use; @@ -422,6 +417,7 @@ static ssize_t audio_pkt_write(struct file *file, const char __user *buf, ret = audpkt_chk_and_update_physical_addr((struct audio_gpr_pkt *) audpkt_hdr); if (ret < 0) { AUDIO_PKT_ERR("Update Physical Address Failed -%d\n", ret); + kfree(kbuf); return ret; } } @@ -457,6 +453,8 @@ static ssize_t audio_pkt_write(struct file *file, const char __user *buf, ret = gpr_send_pkt(audpkt_dev->adev, (struct gpr_pkt *) kbuf); if (ret < 0) { AUDIO_PKT_ERR("APR Send Packet Failed ret -%d\n", ret); + mutex_unlock(&audpkt_dev->lock); + kfree(kbuf); return ret; } mutex_unlock(&audpkt_dev->lock); @@ -481,7 +479,6 @@ static unsigned int audio_pkt_poll(struct file *file, poll_table *wait) unsigned int mask = 0; unsigned long flags; - audpkt_dev = file->private_data; if (!audpkt_dev) { AUDIO_PKT_ERR("invalid device handle\n"); return POLLERR; @@ -585,7 +582,7 @@ static int q6apm_audio_pkt_probe(gpr_device_t *adev) err_device: class_destroy(apm->audio_pkt_class); err_class: - unregister_chrdev_region(MAJOR(apm->audio_pkt_major), + unregister_chrdev_region(apm->audio_pkt_major, MINOR_NUMBER_COUNT); err_chrdev: return ret; @@ -604,15 +601,20 @@ static int q6apm_audio_pkt_callback_core(const struct gpr_resp_pkt *data, void * struct q6apm_audio_pkt *apm = dev_get_drvdata(&gdev->dev); const struct gpr_ibasic_rsp_result_t *result; const struct gpr_hdr *hdr = &data->hdr; - uint8_t *pkt = NULL; + struct device *dev = &gdev->dev; uint16_t hdr_size, pkt_size; unsigned long flags; struct sk_buff *skb; struct gpr_port_map *audpkt_port_map; - u16 new_dest_port = 0, new_src_port = 0; + u32 new_dest_port = 0, new_src_port = 0; bool remap_ports = false; + if (!apm) { + dev_dbg(dev, "callback with NULL drvdata\n"); + return -ENODEV; + } + hdr_size = hdr->hdr_size * 4; pkt_size = hdr->pkt_size; @@ -626,33 +628,24 @@ static int q6apm_audio_pkt_callback_core(const struct gpr_resp_pkt *data, void * idr_remove(&apm->audpkt_port_idr, hdr->token); kfree(audpkt_port_map); } else { - AUDIO_PKT_ERR("Token=%u not found\n", hdr->token); + dev_dbg(dev, "Token=%u not found\n", hdr->token); } mutex_unlock(&apm->audpkt_port_lock); - pkt = kmalloc(pkt_size, GFP_KERNEL); - if (!pkt) - return -ENOMEM; - - memcpy(pkt, (uint8_t *)data, hdr_size); - memcpy(pkt + hdr_size, (uint8_t *)data->payload, pkt_size - hdr_size); - - if (remap_ports) { - struct gpr_hdr *out_hdr = (struct gpr_hdr *)pkt; + skb = alloc_skb(pkt_size, GFP_ATOMIC); + if (!skb) + return -ENOMEM; - out_hdr->dest_port = new_dest_port; - out_hdr->src_port = new_src_port; - } + skb_put_data(skb, (uint8_t *)data, hdr_size); + skb_put_data(skb, (uint8_t *)data->payload, pkt_size - hdr_size); - skb = alloc_skb(pkt_size, GFP_ATOMIC); - if (!skb) { - kfree(pkt); - return -ENOMEM; - } + if (remap_ports) { + struct gpr_hdr *out_hdr = (struct gpr_hdr *)skb->data; - skb_put_data(skb, (void *)pkt, pkt_size); + out_hdr->dest_port = new_dest_port; + out_hdr->src_port = new_src_port; + } - kfree(pkt); spin_lock_irqsave(&apm->queue_lock, flags); skb_queue_tail(&apm->queue, skb); spin_unlock_irqrestore(&apm->queue_lock, flags); @@ -661,19 +654,32 @@ static int q6apm_audio_pkt_callback_core(const struct gpr_resp_pkt *data, void * /* wake up any blocking processes, waiting for new data */ wake_up_interruptible(&apm->readq); if(hdr->opcode == APM_CMD_RSP_GET_SPF_STATE) { - result = data->payload; - apm->result.opcode = hdr->opcode; - apm->result.status = 0; - /* First word of result it state */ - apm->state = hdr->opcode; - } + result = data->payload; + apm->result.opcode = hdr->opcode; + apm->result.status = 0; + /* First word of result it state */ + apm->state = hdr->opcode; + } return 0; } static void q6apm_audio_pkt_remove(gpr_device_t *adev) { - of_platform_depopulate(&adev->dev); + struct device *dev = &adev->dev; + struct q6apm_audio_pkt *apm = dev_get_drvdata(dev); + + of_platform_depopulate(dev); + + if (!apm) + return; + + cdev_del(&apm->cdev); + device_destroy(apm->audio_pkt_class, apm->audio_pkt_major); + class_destroy(apm->audio_pkt_class); + unregister_chrdev_region(apm->audio_pkt_major, MINOR_NUMBER_COUNT); + + dev_set_drvdata(dev, NULL); } #ifdef CONFIG_OF