static int pcs_parse_bits_in_pinctrl_entry()

in pinctrl-single.c [1110:1243]

• 108 lines of code
• 14 McCabe index (conditional complexity)

static int pcs_parse_bits_in_pinctrl_entry(struct pcs_device *pcs,
						struct device_node *np,
						struct pinctrl_map **map,
						unsigned *num_maps,
						const char **pgnames)
{
	const char *name = "pinctrl-single,bits";
	struct pcs_func_vals *vals;
	int rows, *pins, found = 0, res = -ENOMEM, i, fsel;
	int npins_in_row;
	struct pcs_function *function = NULL;

	rows = pinctrl_count_index_with_args(np, name);
	if (rows <= 0) {
		dev_err(pcs->dev, "Invalid number of rows: %d\n", rows);
		return -EINVAL;
	}

	if (PCS_HAS_PINCONF) {
		dev_err(pcs->dev, "pinconf not supported\n");
		return -ENOTSUPP;
	}

	npins_in_row = pcs->width / pcs->bits_per_pin;

	vals = devm_kzalloc(pcs->dev,
			    array3_size(rows, npins_in_row, sizeof(*vals)),
			    GFP_KERNEL);
	if (!vals)
		return -ENOMEM;

	pins = devm_kzalloc(pcs->dev,
			    array3_size(rows, npins_in_row, sizeof(*pins)),
			    GFP_KERNEL);
	if (!pins)
		goto free_vals;

	for (i = 0; i < rows; i++) {
		struct of_phandle_args pinctrl_spec;
		unsigned offset, val;
		unsigned mask, bit_pos, val_pos, mask_pos, submask;
		unsigned pin_num_from_lsb;
		int pin;

		res = pinctrl_parse_index_with_args(np, name, i, &pinctrl_spec);
		if (res)
			return res;

		if (pinctrl_spec.args_count < 3) {
			dev_err(pcs->dev, "invalid args_count for spec: %i\n",
				pinctrl_spec.args_count);
			break;
		}

		/* Index plus two value cells */
		offset = pinctrl_spec.args[0];
		val = pinctrl_spec.args[1];
		mask = pinctrl_spec.args[2];

		dev_dbg(pcs->dev, "%pOFn index: 0x%x value: 0x%x mask: 0x%x\n",
			pinctrl_spec.np, offset, val, mask);

		/* Parse pins in each row from LSB */
		while (mask) {
			bit_pos = __ffs(mask);
			pin_num_from_lsb = bit_pos / pcs->bits_per_pin;
			mask_pos = ((pcs->fmask) << bit_pos);
			val_pos = val & mask_pos;
			submask = mask & mask_pos;

			if ((mask & mask_pos) == 0) {
				dev_err(pcs->dev,
					"Invalid mask for %pOFn at 0x%x\n",
					np, offset);
				break;
			}

			mask &= ~mask_pos;

			if (submask != mask_pos) {
				dev_warn(pcs->dev,
						"Invalid submask 0x%x for %pOFn at 0x%x\n",
						submask, np, offset);
				continue;
			}

			vals[found].mask = submask;
			vals[found].reg = pcs->base + offset;
			vals[found].val = val_pos;

			pin = pcs_get_pin_by_offset(pcs, offset);
			if (pin < 0) {
				dev_err(pcs->dev,
					"could not add functions for %pOFn %ux\n",
					np, offset);
				break;
			}
			pins[found++] = pin + pin_num_from_lsb;
		}
	}

	pgnames[0] = np->name;
	mutex_lock(&pcs->mutex);
	fsel = pcs_add_function(pcs, &function, np->name, vals, found,
				pgnames, 1);
	if (fsel < 0) {
		res = fsel;
		goto free_pins;
	}

	res = pinctrl_generic_add_group(pcs->pctl, np->name, pins, found, pcs);
	if (res < 0)
		goto free_function;

	(*map)->type = PIN_MAP_TYPE_MUX_GROUP;
	(*map)->data.mux.group = np->name;
	(*map)->data.mux.function = np->name;

	*num_maps = 1;
	mutex_unlock(&pcs->mutex);

	return 0;

free_function:
	pinmux_generic_remove_function(pcs->pctl, fsel);
free_pins:
	mutex_unlock(&pcs->mutex);
	devm_kfree(pcs->dev, pins);

free_vals:
	devm_kfree(pcs->dev, vals);

	return res;
}