static int msi_register_init(struct pci_dev *dev, struct msi_desc *entry)
{
int status;
u32 address_hi;
u32 address_lo;
u32 data;
int pos, vector = dev->irq;
u16 control;
pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
pci_read_config_word(dev, msi_control_reg(pos), &control);
/* Configure MSI capability structure */
status = msi_ops->setup(dev, vector, &address_hi, &address_lo, &data);
if (status < 0)
return status;
pci_write_config_dword(dev, msi_lower_address_reg(pos), address_lo);
if (is_64bit_address(control)) {
pci_write_config_dword(dev,
msi_upper_address_reg(pos), address_hi);
pci_write_config_word(dev,
msi_data_reg(pos, 1), data);
} else
pci_write_config_word(dev,
msi_data_reg(pos, 0), 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);
}
return 0;
}
/**
* 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 function 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 = (void __iomem *)(long)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;
}