diff --git a/drivers/ssb/pcmcia.c b/drivers/ssb/pcmcia.c index 100e7a5c5ea1f3a48991845ba1e1cf06758e462c..e72f4046a5e0ee1cccb46a7bd11362261f9f3e0b 100644 --- a/drivers/ssb/pcmcia.c +++ b/drivers/ssb/pcmcia.c @@ -617,136 +617,140 @@ static int ssb_pcmcia_sprom_check_crc(const u16 *sprom, size_t size) } \ } while (0) -int ssb_pcmcia_get_invariants(struct ssb_bus *bus, - struct ssb_init_invariants *iv) +static int ssb_pcmcia_get_mac(struct pcmcia_device *p_dev, + tuple_t *tuple, + void *priv) { - tuple_t tuple; - int res; - unsigned char buf[32]; + struct ssb_sprom *sprom = priv; + + if (tuple->TupleData[0] != CISTPL_FUNCE_LAN_NODE_ID) + return -EINVAL; + if (tuple->TupleDataLen != ETH_ALEN + 2) + return -EINVAL; + if (tuple->TupleData[1] != ETH_ALEN) + return -EINVAL; + memcpy(sprom->il0mac, &tuple->TupleData[2], ETH_ALEN); + return 0; +}; + +static int ssb_pcmcia_do_get_invariants(struct pcmcia_device *p_dev, + tuple_t *tuple, + void *priv) +{ + struct ssb_init_invariants *iv = priv; struct ssb_sprom *sprom = &iv->sprom; struct ssb_boardinfo *bi = &iv->boardinfo; const char *error_description; + GOTO_ERROR_ON(tuple->TupleDataLen < 1, "VEN tpl < 1"); + switch (tuple->TupleData[0]) { + case SSB_PCMCIA_CIS_ID: + GOTO_ERROR_ON((tuple->TupleDataLen != 5) && + (tuple->TupleDataLen != 7), + "id tpl size"); + bi->vendor = tuple->TupleData[1] | + ((u16)tuple->TupleData[2] << 8); + break; + case SSB_PCMCIA_CIS_BOARDREV: + GOTO_ERROR_ON(tuple->TupleDataLen != 2, + "boardrev tpl size"); + sprom->board_rev = tuple->TupleData[1]; + break; + case SSB_PCMCIA_CIS_PA: + GOTO_ERROR_ON((tuple->TupleDataLen != 9) && + (tuple->TupleDataLen != 10), + "pa tpl size"); + sprom->pa0b0 = tuple->TupleData[1] | + ((u16)tuple->TupleData[2] << 8); + sprom->pa0b1 = tuple->TupleData[3] | + ((u16)tuple->TupleData[4] << 8); + sprom->pa0b2 = tuple->TupleData[5] | + ((u16)tuple->TupleData[6] << 8); + sprom->itssi_a = tuple->TupleData[7]; + sprom->itssi_bg = tuple->TupleData[7]; + sprom->maxpwr_a = tuple->TupleData[8]; + sprom->maxpwr_bg = tuple->TupleData[8]; + break; + case SSB_PCMCIA_CIS_OEMNAME: + /* We ignore this. */ + break; + case SSB_PCMCIA_CIS_CCODE: + GOTO_ERROR_ON(tuple->TupleDataLen != 2, + "ccode tpl size"); + sprom->country_code = tuple->TupleData[1]; + break; + case SSB_PCMCIA_CIS_ANTENNA: + GOTO_ERROR_ON(tuple->TupleDataLen != 2, + "ant tpl size"); + sprom->ant_available_a = tuple->TupleData[1]; + sprom->ant_available_bg = tuple->TupleData[1]; + break; + case SSB_PCMCIA_CIS_ANTGAIN: + GOTO_ERROR_ON(tuple->TupleDataLen != 2, + "antg tpl size"); + sprom->antenna_gain.ghz24.a0 = tuple->TupleData[1]; + sprom->antenna_gain.ghz24.a1 = tuple->TupleData[1]; + sprom->antenna_gain.ghz24.a2 = tuple->TupleData[1]; + sprom->antenna_gain.ghz24.a3 = tuple->TupleData[1]; + sprom->antenna_gain.ghz5.a0 = tuple->TupleData[1]; + sprom->antenna_gain.ghz5.a1 = tuple->TupleData[1]; + sprom->antenna_gain.ghz5.a2 = tuple->TupleData[1]; + sprom->antenna_gain.ghz5.a3 = tuple->TupleData[1]; + break; + case SSB_PCMCIA_CIS_BFLAGS: + GOTO_ERROR_ON((tuple->TupleDataLen != 3) && + (tuple->TupleDataLen != 5), + "bfl tpl size"); + sprom->boardflags_lo = tuple->TupleData[1] | + ((u16)tuple->TupleData[2] << 8); + break; + case SSB_PCMCIA_CIS_LEDS: + GOTO_ERROR_ON(tuple->TupleDataLen != 5, + "leds tpl size"); + sprom->gpio0 = tuple->TupleData[1]; + sprom->gpio1 = tuple->TupleData[2]; + sprom->gpio2 = tuple->TupleData[3]; + sprom->gpio3 = tuple->TupleData[4]; + break; + } + return -ENOSPC; /* continue with next entry */ + +error: + ssb_printk(KERN_ERR PFX + "PCMCIA: Failed to fetch device invariants: %s\n", + error_description); + return -ENODEV; +} + + +int ssb_pcmcia_get_invariants(struct ssb_bus *bus, + struct ssb_init_invariants *iv) +{ + struct ssb_sprom *sprom = &iv->sprom; + int res; + memset(sprom, 0xFF, sizeof(*sprom)); sprom->revision = 1; sprom->boardflags_lo = 0; sprom->boardflags_hi = 0; /* First fetch the MAC address. */ - memset(&tuple, 0, sizeof(tuple)); - tuple.DesiredTuple = CISTPL_FUNCE; - tuple.TupleData = buf; - tuple.TupleDataMax = sizeof(buf); - res = pcmcia_get_first_tuple(bus->host_pcmcia, &tuple); - GOTO_ERROR_ON(res != 0, "MAC first tpl"); - res = pcmcia_get_tuple_data(bus->host_pcmcia, &tuple); - GOTO_ERROR_ON(res != 0, "MAC first tpl data"); - while (1) { - GOTO_ERROR_ON(tuple.TupleDataLen < 1, "MAC tpl < 1"); - if (tuple.TupleData[0] == CISTPL_FUNCE_LAN_NODE_ID) - break; - res = pcmcia_get_next_tuple(bus->host_pcmcia, &tuple); - GOTO_ERROR_ON(res != 0, "MAC next tpl"); - res = pcmcia_get_tuple_data(bus->host_pcmcia, &tuple); - GOTO_ERROR_ON(res != 0, "MAC next tpl data"); + res = pcmcia_loop_tuple(bus->host_pcmcia, CISTPL_FUNCE, + ssb_pcmcia_get_mac, sprom); + if (res != 0) { + ssb_printk(KERN_ERR PFX + "PCMCIA: Failed to fetch MAC address\n"); + return -ENODEV; } - GOTO_ERROR_ON(tuple.TupleDataLen != ETH_ALEN + 2, "MAC tpl size"); - memcpy(sprom->il0mac, &tuple.TupleData[2], ETH_ALEN); /* Fetch the vendor specific tuples. */ - memset(&tuple, 0, sizeof(tuple)); - tuple.DesiredTuple = SSB_PCMCIA_CIS; - tuple.TupleData = buf; - tuple.TupleDataMax = sizeof(buf); - res = pcmcia_get_first_tuple(bus->host_pcmcia, &tuple); - GOTO_ERROR_ON(res != 0, "VEN first tpl"); - res = pcmcia_get_tuple_data(bus->host_pcmcia, &tuple); - GOTO_ERROR_ON(res != 0, "VEN first tpl data"); - while (1) { - GOTO_ERROR_ON(tuple.TupleDataLen < 1, "VEN tpl < 1"); - switch (tuple.TupleData[0]) { - case SSB_PCMCIA_CIS_ID: - GOTO_ERROR_ON((tuple.TupleDataLen != 5) && - (tuple.TupleDataLen != 7), - "id tpl size"); - bi->vendor = tuple.TupleData[1] | - ((u16)tuple.TupleData[2] << 8); - break; - case SSB_PCMCIA_CIS_BOARDREV: - GOTO_ERROR_ON(tuple.TupleDataLen != 2, - "boardrev tpl size"); - sprom->board_rev = tuple.TupleData[1]; - break; - case SSB_PCMCIA_CIS_PA: - GOTO_ERROR_ON((tuple.TupleDataLen != 9) && - (tuple.TupleDataLen != 10), - "pa tpl size"); - sprom->pa0b0 = tuple.TupleData[1] | - ((u16)tuple.TupleData[2] << 8); - sprom->pa0b1 = tuple.TupleData[3] | - ((u16)tuple.TupleData[4] << 8); - sprom->pa0b2 = tuple.TupleData[5] | - ((u16)tuple.TupleData[6] << 8); - sprom->itssi_a = tuple.TupleData[7]; - sprom->itssi_bg = tuple.TupleData[7]; - sprom->maxpwr_a = tuple.TupleData[8]; - sprom->maxpwr_bg = tuple.TupleData[8]; - break; - case SSB_PCMCIA_CIS_OEMNAME: - /* We ignore this. */ - break; - case SSB_PCMCIA_CIS_CCODE: - GOTO_ERROR_ON(tuple.TupleDataLen != 2, - "ccode tpl size"); - sprom->country_code = tuple.TupleData[1]; - break; - case SSB_PCMCIA_CIS_ANTENNA: - GOTO_ERROR_ON(tuple.TupleDataLen != 2, - "ant tpl size"); - sprom->ant_available_a = tuple.TupleData[1]; - sprom->ant_available_bg = tuple.TupleData[1]; - break; - case SSB_PCMCIA_CIS_ANTGAIN: - GOTO_ERROR_ON(tuple.TupleDataLen != 2, - "antg tpl size"); - sprom->antenna_gain.ghz24.a0 = tuple.TupleData[1]; - sprom->antenna_gain.ghz24.a1 = tuple.TupleData[1]; - sprom->antenna_gain.ghz24.a2 = tuple.TupleData[1]; - sprom->antenna_gain.ghz24.a3 = tuple.TupleData[1]; - sprom->antenna_gain.ghz5.a0 = tuple.TupleData[1]; - sprom->antenna_gain.ghz5.a1 = tuple.TupleData[1]; - sprom->antenna_gain.ghz5.a2 = tuple.TupleData[1]; - sprom->antenna_gain.ghz5.a3 = tuple.TupleData[1]; - break; - case SSB_PCMCIA_CIS_BFLAGS: - GOTO_ERROR_ON((tuple.TupleDataLen != 3) && - (tuple.TupleDataLen != 5), - "bfl tpl size"); - sprom->boardflags_lo = tuple.TupleData[1] | - ((u16)tuple.TupleData[2] << 8); - break; - case SSB_PCMCIA_CIS_LEDS: - GOTO_ERROR_ON(tuple.TupleDataLen != 5, - "leds tpl size"); - sprom->gpio0 = tuple.TupleData[1]; - sprom->gpio1 = tuple.TupleData[2]; - sprom->gpio2 = tuple.TupleData[3]; - sprom->gpio3 = tuple.TupleData[4]; - break; - } - res = pcmcia_get_next_tuple(bus->host_pcmcia, &tuple); - if (res == -ENOSPC) - break; - GOTO_ERROR_ON(res != 0, "VEN next tpl"); - res = pcmcia_get_tuple_data(bus->host_pcmcia, &tuple); - GOTO_ERROR_ON(res != 0, "VEN next tpl data"); - } + res = pcmcia_loop_tuple(bus->host_pcmcia, SSB_PCMCIA_CIS, + ssb_pcmcia_do_get_invariants, sprom); + if ((res == 0) || (res == -ENOSPC)) + return 0; - return 0; -error: ssb_printk(KERN_ERR PFX - "PCMCIA: Failed to fetch device invariants: %s\n", - error_description); + "PCMCIA: Failed to fetch device invariants\n"); return -ENODEV; }