msi.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 1,148 行 · 第 1/3 页

		status = -EBUSY;
		return status;
	}
	vector_irq[last_alloc_vector] = 0;
	nr_released_vectors++;
	printk(KERN_INFO "MSI INIT SUCCESS\n");

	return status;
}

static int get_msi_vector(struct pci_dev *dev)
{
	return get_new_vector();
}

static struct msi_desc* alloc_msi_entry(void)
{
	struct msi_desc *entry;

	entry = (struct msi_desc*) kmem_cache_alloc(msi_cachep, SLAB_KERNEL);
	if (!entry)
		return NULL;

	memset(entry, 0, sizeof(struct msi_desc));
	entry->link.tail = entry->link.head = 0;	/* single message */
	entry->dev = NULL;

	return entry;
}

static void attach_msi_entry(struct msi_desc *entry, int vector)
{
	unsigned long flags;

	spin_lock_irqsave(&msi_lock, flags);
	msi_desc[vector] = entry;
	spin_unlock_irqrestore(&msi_lock, flags);
}

static void irq_handler_init(int cap_id, int pos, int mask)
{
	spin_lock(&irq_desc[pos].lock);
	if (cap_id == PCI_CAP_ID_MSIX)
		irq_desc[pos].handler = &msix_irq_type;
	else {
		if (!mask)
			irq_desc[pos].handler = &msi_irq_wo_maskbit_type;
		else
			irq_desc[pos].handler = &msi_irq_w_maskbit_type;
	}
	spin_unlock(&irq_desc[pos].lock);
}

static void enable_msi_mode(struct pci_dev *dev, int pos, int type)
{
	u16 control;

	pci_read_config_word(dev, msi_control_reg(pos), &control);
	if (type == PCI_CAP_ID_MSI) {
		/* Set enabled bits to single MSI & enable MSI_enable bit */
		msi_enable(control, 1);
		pci_write_config_word(dev, msi_control_reg(pos), control);
	} else {
		msix_enable(control);
		pci_write_config_word(dev, msi_control_reg(pos), control);
	}
    	if (pci_find_capability(dev, PCI_CAP_ID_EXP)) {
		/* PCI Express Endpoint device detected */
		u16 cmd;
		pci_read_config_word(dev, PCI_COMMAND, &cmd);
		cmd |= PCI_COMMAND_INTX_DISABLE;
		pci_write_config_word(dev, PCI_COMMAND, cmd);
	}
}

static void disable_msi_mode(struct pci_dev *dev, int pos, int type)
{
	u16 control;

	pci_read_config_word(dev, msi_control_reg(pos), &control);
	if (type == PCI_CAP_ID_MSI) {
		/* Set enabled bits to single MSI & enable MSI_enable bit */
		msi_disable(control);
		pci_write_config_word(dev, msi_control_reg(pos), control);
	} else {
		msix_disable(control);
		pci_write_config_word(dev, msi_control_reg(pos), control);
	}
    	if (pci_find_capability(dev, PCI_CAP_ID_EXP)) {
		/* PCI Express Endpoint device detected */
		u16 cmd;
		pci_read_config_word(dev, PCI_COMMAND, &cmd);
		cmd &= ~PCI_COMMAND_INTX_DISABLE;
		pci_write_config_word(dev, PCI_COMMAND, cmd);
	}
}

static int msi_lookup_vector(struct pci_dev *dev, int type)
{
	int vector;
	unsigned long flags;

	spin_lock_irqsave(&msi_lock, flags);
	for (vector = FIRST_DEVICE_VECTOR; vector < NR_IRQS; vector++) {
		if (!msi_desc[vector] || msi_desc[vector]->dev != dev ||
			msi_desc[vector]->msi_attrib.type != type ||
			msi_desc[vector]->msi_attrib.default_vector != dev->irq)
			continue;
		spin_unlock_irqrestore(&msi_lock, flags);
		/* This pre-assigned MSI vector for this device
		   already exits. Override dev->irq with this vector */
		dev->irq = vector;
		return 0;
	}
	spin_unlock_irqrestore(&msi_lock, flags);

	return -EACCES;
}

void pci_scan_msi_device(struct pci_dev *dev)
{
	if (!dev)
		return;

   	if (pci_find_capability(dev, PCI_CAP_ID_MSIX) > 0)
		nr_msix_devices++;
	else if (pci_find_capability(dev, PCI_CAP_ID_MSI) > 0)
		nr_reserved_vectors++;
}

/**
 * msi_capability_init - configure device's MSI capability structure
 * @dev: pointer to the pci_dev data structure of MSI device function
 *
 * Setup the MSI capability structure of device funtion with a single
 * MSI vector, regardless of device function is capable of handling
 * multiple messages. A return of zero indicates the successful setup
 * of an entry zero with the new MSI vector or non-zero for otherwise.
 **/
static int msi_capability_init(struct pci_dev *dev)
{
	struct msi_desc *entry;
	struct msg_address address;
	struct msg_data data;
	int pos, vector;
	u16 control;

   	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
	pci_read_config_word(dev, msi_control_reg(pos), &control);
	/* MSI Entry Initialization */
	if (!(entry = alloc_msi_entry()))
		return -ENOMEM;

	if ((vector = get_msi_vector(dev)) < 0) {
		kmem_cache_free(msi_cachep, entry);
		return -EBUSY;
	}
	entry->link.head = vector;
	entry->link.tail = vector;
	entry->msi_attrib.type = PCI_CAP_ID_MSI;
	entry->msi_attrib.state = 0;			/* Mark it not active */
	entry->msi_attrib.entry_nr = 0;
	entry->msi_attrib.maskbit = is_mask_bit_support(control);
	entry->msi_attrib.default_vector = dev->irq;	/* Save IOAPIC IRQ */
	dev->irq = vector;
	entry->dev = dev;
	if (is_mask_bit_support(control)) {
		entry->mask_base = msi_mask_bits_reg(pos,
				is_64bit_address(control));
	}
	/* Replace with MSI handler */
	irq_handler_init(PCI_CAP_ID_MSI, vector, entry->msi_attrib.maskbit);
	/* Configure MSI capability structure */
	msi_address_init(&address);
	msi_data_init(&data, vector);
	entry->msi_attrib.current_cpu = ((address.lo_address.u.dest_id >>
				MSI_TARGET_CPU_SHIFT) & MSI_TARGET_CPU_MASK);
	pci_write_config_dword(dev, msi_lower_address_reg(pos),
			address.lo_address.value);
	if (is_64bit_address(control)) {
		pci_write_config_dword(dev,
			msi_upper_address_reg(pos), address.hi_address);
		pci_write_config_word(dev,
			msi_data_reg(pos, 1), *((u32*)&data));
	} else
		pci_write_config_word(dev,
			msi_data_reg(pos, 0), *((u32*)&data));
	if (entry->msi_attrib.maskbit) {
		unsigned int maskbits, temp;
		/* All MSIs are unmasked by default, Mask them all */
		pci_read_config_dword(dev,
			msi_mask_bits_reg(pos, is_64bit_address(control)),
			&maskbits);
		temp = (1 << multi_msi_capable(control));
		temp = ((temp - 1) & ~temp);
		maskbits |= temp;
		pci_write_config_dword(dev,
			msi_mask_bits_reg(pos, is_64bit_address(control)),
			maskbits);
	}
	attach_msi_entry(entry, vector);
	/* Set MSI enabled bits	 */
	enable_msi_mode(dev, pos, PCI_CAP_ID_MSI);

	return 0;
}

/**
 * msix_capability_init - configure device's MSI-X capability
 * @dev: pointer to the pci_dev data structure of MSI-X device function
 *
 * Setup the MSI-X capability structure of device funtion with a
 * single MSI-X vector. A return of zero indicates the successful setup of
 * requested MSI-X entries with allocated vectors or non-zero for otherwise.
 **/
static int msix_capability_init(struct pci_dev *dev,
				struct msix_entry *entries, int nvec)
{
	struct msi_desc *head = NULL, *tail = NULL, *entry = NULL;
	struct msg_address address;
	struct msg_data data;
	int vector, pos, i, j, nr_entries, temp = 0;
	u32 phys_addr, table_offset;
 	u16 control;
	u8 bir;
	void *base;

   	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
	/* Request & Map MSI-X table region */
 	pci_read_config_word(dev, msi_control_reg(pos), &control);
	nr_entries = multi_msix_capable(control);
 	pci_read_config_dword(dev, msix_table_offset_reg(pos),
 		&table_offset);
	bir = (u8)(table_offset & PCI_MSIX_FLAGS_BIRMASK);
	phys_addr = pci_resource_start (dev, bir);
	phys_addr += (u32)(table_offset & ~PCI_MSIX_FLAGS_BIRMASK);
	if (!request_mem_region(phys_addr,
		nr_entries * PCI_MSIX_ENTRY_SIZE,
		"MSI-X vector table"))
		return -ENOMEM;
	base = ioremap_nocache(phys_addr, nr_entries * PCI_MSIX_ENTRY_SIZE);
	if (base == NULL) {
		release_mem_region(phys_addr, nr_entries * PCI_MSIX_ENTRY_SIZE);
		return -ENOMEM;
	}
	/* MSI-X Table Initialization */
	for (i = 0; i < nvec; i++) {
		entry = alloc_msi_entry();
		if (!entry)
			break;
		if ((vector = get_msi_vector(dev)) < 0)
			break;

 		j = entries[i].entry;
 		entries[i].vector = vector;
		entry->msi_attrib.type = PCI_CAP_ID_MSIX;
 		entry->msi_attrib.state = 0;		/* Mark it not active */
		entry->msi_attrib.entry_nr = j;
		entry->msi_attrib.maskbit = 1;
		entry->msi_attrib.default_vector = dev->irq;
		entry->dev = dev;
		entry->mask_base = (unsigned long)base;
		if (!head) {
			entry->link.head = vector;
			entry->link.tail = vector;
			head = entry;
		} else {
			entry->link.head = temp;
			entry->link.tail = tail->link.tail;
			tail->link.tail = vector;
			head->link.head = vector;
		}
		temp = vector;
		tail = entry;
		/* Replace with MSI-X handler */
		irq_handler_init(PCI_CAP_ID_MSIX, vector, 1);
		/* Configure MSI-X capability structure */
		msi_address_init(&address);
		msi_data_init(&data, vector);
		entry->msi_attrib.current_cpu =
			((address.lo_address.u.dest_id >>
			MSI_TARGET_CPU_SHIFT) & MSI_TARGET_CPU_MASK);
		writel(address.lo_address.value,
			base + j * PCI_MSIX_ENTRY_SIZE +
			PCI_MSIX_ENTRY_LOWER_ADDR_OFFSET);
		writel(address.hi_address,
			base + j * PCI_MSIX_ENTRY_SIZE +
			PCI_MSIX_ENTRY_UPPER_ADDR_OFFSET);
		writel(*(u32*)&data,
			base + j * PCI_MSIX_ENTRY_SIZE +
			PCI_MSIX_ENTRY_DATA_OFFSET);
		attach_msi_entry(entry, vector);
	}
	if (i != nvec) {
		i--;
		for (; i >= 0; i--) {
			vector = (entries + i)->vector;
			msi_free_vector(dev, vector, 0);
			(entries + i)->vector = 0;
		}
		return -EBUSY;
	}
	/* Set MSI-X enabled bits */
	enable_msi_mode(dev, pos, PCI_CAP_ID_MSIX);

	return 0;
}

/**
 * pci_enable_msi - configure device's MSI capability structure
 * @dev: pointer to the pci_dev data structure of MSI device function
 *
 * Setup the MSI capability structure of device function with
 * a single MSI vector upon its software driver call to request for
 * MSI mode enabled on its hardware device function. A return of zero
 * indicates the successful setup of an entry zero with the new MSI
 * vector or non-zero for otherwise.
 **/
int pci_enable_msi(struct pci_dev* dev)
{
	int pos, temp = dev->irq, status = -EINVAL;
	u16 control;

	if (!pci_msi_enable || !dev)
 		return status;

	if ((status = msi_init()) < 0)
		return status;

   	if (!(pos = pci_find_capability(dev, PCI_CAP_ID_MSI)))
		return -EINVAL;

	pci_read_config_word(dev, msi_control_reg(pos), &control);
	if (control & PCI_MSI_FLAGS_ENABLE)
		return 0;			/* Already in MSI mode */

	if (!msi_lookup_vector(dev, PCI_CAP_ID_MSI)) {
		/* Lookup Sucess */
		unsigned long flags;

		spin_lock_irqsave(&msi_lock, flags);
		if (!vector_irq[dev->irq]) {
			msi_desc[dev->irq]->msi_attrib.state = 0;
			vector_irq[dev->irq] = -1;
			nr_released_vectors--;
			spin_unlock_irqrestore(&msi_lock, flags);
			enable_msi_mode(dev, pos, PCI_CAP_ID_MSI);
			return 0;
		}
		spin_unlock_irqrestore(&msi_lock, flags);
		dev->irq = temp;
	}
	/* Check whether driver already requested for MSI-X vectors */
   	if ((pos = pci_find_capability(dev, PCI_CAP_ID_MSIX)) > 0 &&
		!msi_lookup_vector(dev, PCI_CAP_ID_MSIX)) {
			printk(KERN_INFO "Can't enable MSI. Device already had MSI-X vectors assigned\n");
			dev->irq = temp;
			return -EINVAL;
	}
	status = msi_capability_init(dev);
	if (!status) {
   		if (!pos)
			nr_reserved_vectors--;	/* Only MSI capable */
		else if (nr_msix_devices > 0)
			nr_msix_devices--;	/* Both MSI and MSI-X capable,
						   but choose enabling MSI */
	}

	return status;
}

void pci_disable_msi(struct pci_dev* dev)
{
	struct msi_desc *entry;
	int pos, default_vector;
	u16 control;
	unsigned long flags;

   	if (!dev || !(pos = pci_find_capability(dev, PCI_CAP_ID_MSI)))
		return;

	pci_read_config_word(dev, msi_control_reg(pos), &control);
	if (!(control & PCI_MSI_FLAGS_ENABLE))