static int rtsx_pci_probe()

in cardreader/rtsx_pcr.c [1526:1664]

• 117 lines of code
• 19 McCabe index (conditional complexity)

static int rtsx_pci_probe(struct pci_dev *pcidev,
			  const struct pci_device_id *id)
{
	struct rtsx_pcr *pcr;
	struct pcr_handle *handle;
	u32 base, len;
	int ret, i, bar = 0;

	dev_dbg(&(pcidev->dev),
		": Realtek PCI-E Card Reader found at %s [%04x:%04x] (rev %x)\n",
		pci_name(pcidev), (int)pcidev->vendor, (int)pcidev->device,
		(int)pcidev->revision);

	ret = dma_set_mask(&pcidev->dev, DMA_BIT_MASK(32));
	if (ret < 0)
		return ret;

	ret = pci_enable_device(pcidev);
	if (ret)
		return ret;

	ret = pci_request_regions(pcidev, DRV_NAME_RTSX_PCI);
	if (ret)
		goto disable;

	pcr = kzalloc(sizeof(*pcr), GFP_KERNEL);
	if (!pcr) {
		ret = -ENOMEM;
		goto release_pci;
	}

	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
	if (!handle) {
		ret = -ENOMEM;
		goto free_pcr;
	}
	handle->pcr = pcr;

	idr_preload(GFP_KERNEL);
	spin_lock(&rtsx_pci_lock);
	ret = idr_alloc(&rtsx_pci_idr, pcr, 0, 0, GFP_NOWAIT);
	if (ret >= 0)
		pcr->id = ret;
	spin_unlock(&rtsx_pci_lock);
	idr_preload_end();
	if (ret < 0)
		goto free_handle;

	pcr->pci = pcidev;
	dev_set_drvdata(&pcidev->dev, handle);

	if (CHK_PCI_PID(pcr, 0x525A))
		bar = 1;
	len = pci_resource_len(pcidev, bar);
	base = pci_resource_start(pcidev, bar);
	pcr->remap_addr = ioremap(base, len);
	if (!pcr->remap_addr) {
		ret = -ENOMEM;
		goto free_handle;
	}

	pcr->rtsx_resv_buf = dma_alloc_coherent(&(pcidev->dev),
			RTSX_RESV_BUF_LEN, &(pcr->rtsx_resv_buf_addr),
			GFP_KERNEL);
	if (pcr->rtsx_resv_buf == NULL) {
		ret = -ENXIO;
		goto unmap;
	}
	pcr->host_cmds_ptr = pcr->rtsx_resv_buf;
	pcr->host_cmds_addr = pcr->rtsx_resv_buf_addr;
	pcr->host_sg_tbl_ptr = pcr->rtsx_resv_buf + HOST_CMDS_BUF_LEN;
	pcr->host_sg_tbl_addr = pcr->rtsx_resv_buf_addr + HOST_CMDS_BUF_LEN;
	pcr->card_inserted = 0;
	pcr->card_removed = 0;
	INIT_DELAYED_WORK(&pcr->carddet_work, rtsx_pci_card_detect);
	INIT_DELAYED_WORK(&pcr->idle_work, rtsx_pci_idle_work);

	pcr->msi_en = msi_en;
	if (pcr->msi_en) {
		ret = pci_enable_msi(pcidev);
		if (ret)
			pcr->msi_en = false;
	}

	ret = rtsx_pci_acquire_irq(pcr);
	if (ret < 0)
		goto disable_msi;

	pci_set_master(pcidev);
	synchronize_irq(pcr->irq);

	ret = rtsx_pci_init_chip(pcr);
	if (ret < 0)
		goto disable_irq;

	for (i = 0; i < ARRAY_SIZE(rtsx_pcr_cells); i++) {
		rtsx_pcr_cells[i].platform_data = handle;
		rtsx_pcr_cells[i].pdata_size = sizeof(*handle);
	}

	if (pcr->rtd3_en) {
		INIT_DELAYED_WORK(&pcr->rtd3_work, rtsx_pci_rtd3_work);
		pm_runtime_allow(&pcidev->dev);
		pm_runtime_enable(&pcidev->dev);
		pcr->is_runtime_suspended = false;
	}


	ret = mfd_add_devices(&pcidev->dev, pcr->id, rtsx_pcr_cells,
			ARRAY_SIZE(rtsx_pcr_cells), NULL, 0, NULL);
	if (ret < 0)
		goto free_slots;

	schedule_delayed_work(&pcr->idle_work, msecs_to_jiffies(200));

	return 0;

free_slots:
	kfree(pcr->slots);
disable_irq:
	free_irq(pcr->irq, (void *)pcr);
disable_msi:
	if (pcr->msi_en)
		pci_disable_msi(pcr->pci);
	dma_free_coherent(&(pcr->pci->dev), RTSX_RESV_BUF_LEN,
			pcr->rtsx_resv_buf, pcr->rtsx_resv_buf_addr);
unmap:
	iounmap(pcr->remap_addr);
free_handle:
	kfree(handle);
free_pcr:
	kfree(pcr);
release_pci:
	pci_release_regions(pcidev);
disable:
	pci_disable_device(pcidev);

	return ret;
}