in regmap/regmap-irq.c [628:918]
int regmap_add_irq_chip_fwnode(struct fwnode_handle *fwnode,
struct regmap *map, int irq,
int irq_flags, int irq_base,
const struct regmap_irq_chip *chip,
struct regmap_irq_chip_data **data)
{
struct regmap_irq_chip_data *d;
int i;
int ret = -ENOMEM;
int num_type_reg;
u32 reg;
u32 unmask_offset;
if (chip->num_regs <= 0)
return -EINVAL;
if (chip->clear_on_unmask && (chip->ack_base || chip->use_ack))
return -EINVAL;
for (i = 0; i < chip->num_irqs; i++) {
if (chip->irqs[i].reg_offset % map->reg_stride)
return -EINVAL;
if (chip->irqs[i].reg_offset / map->reg_stride >=
chip->num_regs)
return -EINVAL;
}
if (chip->not_fixed_stride) {
for (i = 0; i < chip->num_regs; i++)
if (chip->sub_reg_offsets[i].num_regs != 1)
return -EINVAL;
}
if (irq_base) {
irq_base = irq_alloc_descs(irq_base, 0, chip->num_irqs, 0);
if (irq_base < 0) {
dev_warn(map->dev, "Failed to allocate IRQs: %d\n",
irq_base);
return irq_base;
}
}
d = kzalloc(sizeof(*d), GFP_KERNEL);
if (!d)
return -ENOMEM;
if (chip->num_main_regs) {
d->main_status_buf = kcalloc(chip->num_main_regs,
sizeof(unsigned int),
GFP_KERNEL);
if (!d->main_status_buf)
goto err_alloc;
}
d->status_buf = kcalloc(chip->num_regs, sizeof(unsigned int),
GFP_KERNEL);
if (!d->status_buf)
goto err_alloc;
d->mask_buf = kcalloc(chip->num_regs, sizeof(unsigned int),
GFP_KERNEL);
if (!d->mask_buf)
goto err_alloc;
d->mask_buf_def = kcalloc(chip->num_regs, sizeof(unsigned int),
GFP_KERNEL);
if (!d->mask_buf_def)
goto err_alloc;
if (chip->wake_base) {
d->wake_buf = kcalloc(chip->num_regs, sizeof(unsigned int),
GFP_KERNEL);
if (!d->wake_buf)
goto err_alloc;
}
num_type_reg = chip->type_in_mask ? chip->num_regs : chip->num_type_reg;
if (num_type_reg) {
d->type_buf_def = kcalloc(num_type_reg,
sizeof(unsigned int), GFP_KERNEL);
if (!d->type_buf_def)
goto err_alloc;
d->type_buf = kcalloc(num_type_reg, sizeof(unsigned int),
GFP_KERNEL);
if (!d->type_buf)
goto err_alloc;
}
if (chip->num_virt_regs) {
/*
* Create virt_buf[chip->num_extra_config_regs][chip->num_regs]
*/
d->virt_buf = kcalloc(chip->num_virt_regs, sizeof(*d->virt_buf),
GFP_KERNEL);
if (!d->virt_buf)
goto err_alloc;
for (i = 0; i < chip->num_virt_regs; i++) {
d->virt_buf[i] = kcalloc(chip->num_regs,
sizeof(unsigned int),
GFP_KERNEL);
if (!d->virt_buf[i])
goto err_alloc;
}
}
d->irq_chip = regmap_irq_chip;
d->irq_chip.name = chip->name;
d->irq = irq;
d->map = map;
d->chip = chip;
d->irq_base = irq_base;
if (chip->irq_reg_stride)
d->irq_reg_stride = chip->irq_reg_stride;
else
d->irq_reg_stride = 1;
if (chip->type_reg_stride)
d->type_reg_stride = chip->type_reg_stride;
else
d->type_reg_stride = 1;
if (!map->use_single_read && map->reg_stride == 1 &&
d->irq_reg_stride == 1) {
d->status_reg_buf = kmalloc_array(chip->num_regs,
map->format.val_bytes,
GFP_KERNEL);
if (!d->status_reg_buf)
goto err_alloc;
}
mutex_init(&d->lock);
for (i = 0; i < chip->num_irqs; i++)
d->mask_buf_def[chip->irqs[i].reg_offset / map->reg_stride]
|= chip->irqs[i].mask;
/* Mask all the interrupts by default */
for (i = 0; i < chip->num_regs; i++) {
d->mask_buf[i] = d->mask_buf_def[i];
if (!chip->mask_base)
continue;
reg = sub_irq_reg(d, d->chip->mask_base, i);
if (chip->mask_invert)
ret = regmap_irq_update_bits(d, reg,
d->mask_buf[i], ~d->mask_buf[i]);
else if (d->chip->unmask_base) {
unmask_offset = d->chip->unmask_base -
d->chip->mask_base;
ret = regmap_irq_update_bits(d,
reg + unmask_offset,
d->mask_buf[i],
d->mask_buf[i]);
} else
ret = regmap_irq_update_bits(d, reg,
d->mask_buf[i], d->mask_buf[i]);
if (ret != 0) {
dev_err(map->dev, "Failed to set masks in 0x%x: %d\n",
reg, ret);
goto err_alloc;
}
if (!chip->init_ack_masked)
continue;
/* Ack masked but set interrupts */
reg = sub_irq_reg(d, d->chip->status_base, i);
ret = regmap_read(map, reg, &d->status_buf[i]);
if (ret != 0) {
dev_err(map->dev, "Failed to read IRQ status: %d\n",
ret);
goto err_alloc;
}
if (chip->status_invert)
d->status_buf[i] = ~d->status_buf[i];
if (d->status_buf[i] && (chip->ack_base || chip->use_ack)) {
reg = sub_irq_reg(d, d->chip->ack_base, i);
if (chip->ack_invert)
ret = regmap_write(map, reg,
~(d->status_buf[i] & d->mask_buf[i]));
else
ret = regmap_write(map, reg,
d->status_buf[i] & d->mask_buf[i]);
if (chip->clear_ack) {
if (chip->ack_invert && !ret)
ret = regmap_write(map, reg,
(d->status_buf[i] &
d->mask_buf[i]));
else if (!ret)
ret = regmap_write(map, reg,
~(d->status_buf[i] &
d->mask_buf[i]));
}
if (ret != 0) {
dev_err(map->dev, "Failed to ack 0x%x: %d\n",
reg, ret);
goto err_alloc;
}
}
}
/* Wake is disabled by default */
if (d->wake_buf) {
for (i = 0; i < chip->num_regs; i++) {
d->wake_buf[i] = d->mask_buf_def[i];
reg = sub_irq_reg(d, d->chip->wake_base, i);
if (chip->wake_invert)
ret = regmap_irq_update_bits(d, reg,
d->mask_buf_def[i],
0);
else
ret = regmap_irq_update_bits(d, reg,
d->mask_buf_def[i],
d->wake_buf[i]);
if (ret != 0) {
dev_err(map->dev, "Failed to set masks in 0x%x: %d\n",
reg, ret);
goto err_alloc;
}
}
}
if (chip->num_type_reg && !chip->type_in_mask) {
for (i = 0; i < chip->num_type_reg; ++i) {
reg = sub_irq_reg(d, d->chip->type_base, i);
ret = regmap_read(map, reg, &d->type_buf_def[i]);
if (d->chip->type_invert)
d->type_buf_def[i] = ~d->type_buf_def[i];
if (ret) {
dev_err(map->dev, "Failed to get type defaults at 0x%x: %d\n",
reg, ret);
goto err_alloc;
}
}
}
if (irq_base)
d->domain = irq_domain_create_legacy(fwnode, chip->num_irqs,
irq_base, 0,
®map_domain_ops, d);
else
d->domain = irq_domain_create_linear(fwnode, chip->num_irqs,
®map_domain_ops, d);
if (!d->domain) {
dev_err(map->dev, "Failed to create IRQ domain\n");
ret = -ENOMEM;
goto err_alloc;
}
ret = request_threaded_irq(irq, NULL, regmap_irq_thread,
irq_flags | IRQF_ONESHOT,
chip->name, d);
if (ret != 0) {
dev_err(map->dev, "Failed to request IRQ %d for %s: %d\n",
irq, chip->name, ret);
goto err_domain;
}
*data = d;
return 0;
err_domain:
/* Should really dispose of the domain but... */
err_alloc:
kfree(d->type_buf);
kfree(d->type_buf_def);
kfree(d->wake_buf);
kfree(d->mask_buf_def);
kfree(d->mask_buf);
kfree(d->status_buf);
kfree(d->status_reg_buf);
if (d->virt_buf) {
for (i = 0; i < chip->num_virt_regs; i++)
kfree(d->virt_buf[i]);
kfree(d->virt_buf);
}
kfree(d);
return ret;
}