| 1 | --- a/drivers/bcma/driver_pci_host.c |
| 2 | +++ b/drivers/bcma/driver_pci_host.c |
| 3 | @@ -94,19 +94,19 @@ static int bcma_extpci_read_config(struc |
| 4 | if (dev == 0) { |
| 5 | /* we support only two functions on device 0 */ |
| 6 | if (func > 1) |
| 7 | - return -EINVAL; |
| 8 | + goto out; |
| 9 | |
| 10 | /* accesses to config registers with offsets >= 256 |
| 11 | * requires indirect access. |
| 12 | */ |
| 13 | if (off >= PCI_CONFIG_SPACE_SIZE) { |
| 14 | addr = (func << 12); |
| 15 | - addr |= (off & 0x0FFF); |
| 16 | + addr |= (off & 0x0FFC); |
| 17 | val = bcma_pcie_read_config(pc, addr); |
| 18 | } else { |
| 19 | addr = BCMA_CORE_PCI_PCICFG0; |
| 20 | addr |= (func << 8); |
| 21 | - addr |= (off & 0xfc); |
| 22 | + addr |= (off & 0xFC); |
| 23 | val = pcicore_read32(pc, addr); |
| 24 | } |
| 25 | } else { |
| 26 | @@ -122,8 +122,6 @@ static int bcma_extpci_read_config(struc |
| 27 | val = 0xffffffff; |
| 28 | goto unmap; |
| 29 | } |
| 30 | - |
| 31 | - val = readl(mmio); |
| 32 | } |
| 33 | val >>= (8 * (off & 3)); |
| 34 | |
| 35 | @@ -151,7 +149,7 @@ static int bcma_extpci_write_config(stru |
| 36 | const void *buf, int len) |
| 37 | { |
| 38 | int err = -EINVAL; |
| 39 | - u32 addr = 0, val = 0; |
| 40 | + u32 addr, val; |
| 41 | void __iomem *mmio = 0; |
| 42 | u16 chipid = pc->core->bus->chipinfo.id; |
| 43 | |
| 44 | @@ -159,16 +157,22 @@ static int bcma_extpci_write_config(stru |
| 45 | if (unlikely(len != 1 && len != 2 && len != 4)) |
| 46 | goto out; |
| 47 | if (dev == 0) { |
| 48 | + /* we support only two functions on device 0 */ |
| 49 | + if (func > 1) |
| 50 | + goto out; |
| 51 | + |
| 52 | /* accesses to config registers with offsets >= 256 |
| 53 | * requires indirect access. |
| 54 | */ |
| 55 | - if (off < PCI_CONFIG_SPACE_SIZE) { |
| 56 | - addr = pc->core->addr + BCMA_CORE_PCI_PCICFG0; |
| 57 | + if (off >= PCI_CONFIG_SPACE_SIZE) { |
| 58 | + addr = (func << 12); |
| 59 | + addr |= (off & 0x0FFC); |
| 60 | + val = bcma_pcie_read_config(pc, addr); |
| 61 | + } else { |
| 62 | + addr = BCMA_CORE_PCI_PCICFG0; |
| 63 | addr |= (func << 8); |
| 64 | - addr |= (off & 0xfc); |
| 65 | - mmio = ioremap_nocache(addr, sizeof(val)); |
| 66 | - if (!mmio) |
| 67 | - goto out; |
| 68 | + addr |= (off & 0xFC); |
| 69 | + val = pcicore_read32(pc, addr); |
| 70 | } |
| 71 | } else { |
| 72 | addr = bcma_get_cfgspace_addr(pc, dev, func, off); |
| 73 | @@ -187,12 +191,10 @@ static int bcma_extpci_write_config(stru |
| 74 | |
| 75 | switch (len) { |
| 76 | case 1: |
| 77 | - val = readl(mmio); |
| 78 | val &= ~(0xFF << (8 * (off & 3))); |
| 79 | val |= *((const u8 *)buf) << (8 * (off & 3)); |
| 80 | break; |
| 81 | case 2: |
| 82 | - val = readl(mmio); |
| 83 | val &= ~(0xFFFF << (8 * (off & 3))); |
| 84 | val |= *((const u16 *)buf) << (8 * (off & 3)); |
| 85 | break; |
| 86 | @@ -200,13 +202,14 @@ static int bcma_extpci_write_config(stru |
| 87 | val = *((const u32 *)buf); |
| 88 | break; |
| 89 | } |
| 90 | - if (dev == 0 && !addr) { |
| 91 | + if (dev == 0) { |
| 92 | /* accesses to config registers with offsets >= 256 |
| 93 | * requires indirect access. |
| 94 | */ |
| 95 | - addr = (func << 12); |
| 96 | - addr |= (off & 0x0FFF); |
| 97 | - bcma_pcie_write_config(pc, addr, val); |
| 98 | + if (off >= PCI_CONFIG_SPACE_SIZE) |
| 99 | + bcma_pcie_write_config(pc, addr, val); |
| 100 | + else |
| 101 | + pcicore_write32(pc, addr, val); |
| 102 | } else { |
| 103 | writel(val, mmio); |
| 104 | |
| 105 | |
