Re: [PATCH v6 4/5] net: wan: fsl_qmc_hdlc: Add runtime timeslots changes support

[Yury Norov <yury.norov@gmail.com>]

On Wed, Mar 06, 2024 at 09:07:20AM +0100, Herve Codina wrote:
> QMC channels support runtime timeslots changes but nothing is done at
> the QMC HDLC driver to handle these changes.
> 
> Use existing IFACE ioctl in order to configure the timeslots to use.
> 
> Signed-off-by: Herve Codina <herve.codina@bootlin.com>
> Reviewed-by: Christophe Leroy <christophe.leroy@csgroup.eu>
> Acked-by: Jakub Kicinski <kuba@kernel.org>
> Reviewed-by: Andy Shevchenko <andriy.shevchenko@linux.intel.com>
> ---
>  drivers/net/wan/fsl_qmc_hdlc.c | 151 ++++++++++++++++++++++++++++++++-
>  1 file changed, 150 insertions(+), 1 deletion(-)
> 
> diff --git a/drivers/net/wan/fsl_qmc_hdlc.c b/drivers/net/wan/fsl_qmc_hdlc.c
> index 90063a92209e..31c0f32474a3 100644
> --- a/drivers/net/wan/fsl_qmc_hdlc.c
> +++ b/drivers/net/wan/fsl_qmc_hdlc.c
> @@ -10,6 +10,7 @@
>  #include <linux/array_size.h>
>  #include <linux/bug.h>
>  #include <linux/cleanup.h>
> +#include <linux/bitmap.h>
>  #include <linux/dma-mapping.h>
>  #include <linux/device.h>
>  #include <linux/err.h>
> @@ -39,6 +40,7 @@ struct qmc_hdlc {
>  	struct qmc_hdlc_desc tx_descs[8];
>  	unsigned int tx_out;
>  	struct qmc_hdlc_desc rx_descs[4];
> +	u32 slot_map;
>  };
>  
>  static struct qmc_hdlc *netdev_to_qmc_hdlc(struct net_device *netdev)
> @@ -203,6 +205,144 @@ static netdev_tx_t qmc_hdlc_xmit(struct sk_buff *skb, struct net_device *netdev)
>  	return NETDEV_TX_OK;
>  }
>  
> +static int qmc_hdlc_xlate_slot_map(struct qmc_hdlc *qmc_hdlc,
> +				   u32 slot_map, struct qmc_chan_ts_info *ts_info)
> +{
> +	DECLARE_BITMAP(ts_mask_avail, 64);
> +	DECLARE_BITMAP(ts_mask, 64);
> +	DECLARE_BITMAP(map, 64);
> +
> +	/* Tx and Rx available masks must be identical */
> +	if (ts_info->rx_ts_mask_avail != ts_info->tx_ts_mask_avail) {
> +		dev_err(qmc_hdlc->dev, "tx and rx available timeslots mismatch (0x%llx, 0x%llx)\n",
> +			ts_info->rx_ts_mask_avail, ts_info->tx_ts_mask_avail);
> +		return -EINVAL;
> +	}
> +
> +	bitmap_from_u64(ts_mask_avail, ts_info->rx_ts_mask_avail);
> +	bitmap_from_u64(map, slot_map);
> +	bitmap_scatter(ts_mask, map, ts_mask_avail, 64);

We've got a BITMAP_FROM_U64() for this:

	DECLARE_BITMAP(ts_mask_avail, 64) = { BITMAP_FROM_U64(ts_info->rx_ts_mask_avail) };
	DECLARE_BITMAP(map, 64) = { BITMAP_FROM_U64(slot_map) };

> +
> +	if (bitmap_weight(ts_mask, 64) != bitmap_weight(map, 64)) {
> +		dev_err(qmc_hdlc->dev, "Cannot translate timeslots %64pb -> (%64pb, %64pb)\n",
> +			map, ts_mask_avail, ts_mask);
> +		return -EINVAL;
> +	}
> +
> +	bitmap_to_arr64(&ts_info->tx_ts_mask, ts_mask, 64);
> +	ts_info->rx_ts_mask = ts_info->tx_ts_mask;
> +	return 0;
> +}
> +
> +static int qmc_hdlc_xlate_ts_info(struct qmc_hdlc *qmc_hdlc,
> +				  const struct qmc_chan_ts_info *ts_info, u32 *slot_map)
> +{
> +	DECLARE_BITMAP(ts_mask_avail, 64);
> +	DECLARE_BITMAP(ts_mask, 64);
> +	DECLARE_BITMAP(map, 64);
> +	u32 array32[2];

NIT. Bad name. I'd suggest slot_array, or something.

> +	/* Tx and Rx masks and available masks must be identical */
> +	if (ts_info->rx_ts_mask_avail != ts_info->tx_ts_mask_avail) {
> +		dev_err(qmc_hdlc->dev, "tx and rx available timeslots mismatch (0x%llx, 0x%llx)\n",
> +			ts_info->rx_ts_mask_avail, ts_info->tx_ts_mask_avail);
> +		return -EINVAL;
> +	}
> +	if (ts_info->rx_ts_mask != ts_info->tx_ts_mask) {
> +		dev_err(qmc_hdlc->dev, "tx and rx timeslots mismatch (0x%llx, 0x%llx)\n",
> +			ts_info->rx_ts_mask, ts_info->tx_ts_mask);
> +		return -EINVAL;
> +	}
> +
> +	bitmap_from_u64(ts_mask_avail, ts_info->rx_ts_mask_avail);
> +	bitmap_from_u64(ts_mask, ts_info->rx_ts_mask);

Same as above, can you try using BITMAP_FROM_U64()?

Thanks,
Yury

> +	bitmap_gather(map, ts_mask, ts_mask_avail, 64);
> +
> +	if (bitmap_weight(ts_mask, 64) != bitmap_weight(map, 64)) {
> +		dev_err(qmc_hdlc->dev, "Cannot translate timeslots (%64pb, %64pb) -> %64pb\n",
> +			ts_mask_avail, ts_mask, map);
> +		return -EINVAL;
> +	}
> +
> +	bitmap_to_arr32(array32, map, 64);
> +	if (array32[1]) {
> +		dev_err(qmc_hdlc->dev, "Slot map out of 32bit (%64pb, %64pb) -> %64pb\n",
> +			ts_mask_avail, ts_mask, map);
> +		return -EINVAL;
> +	}
> +
> +	*slot_map = array32[0];
> +	return 0;
> +}
> +
> +static int qmc_hdlc_set_iface(struct qmc_hdlc *qmc_hdlc, int if_iface, const te1_settings *te1)
> +{
> +	struct qmc_chan_ts_info ts_info;
> +	int ret;
> +
> +	ret = qmc_chan_get_ts_info(qmc_hdlc->qmc_chan, &ts_info);
> +	if (ret) {
> +		dev_err(qmc_hdlc->dev, "get QMC channel ts info failed %d\n", ret);
> +		return ret;
> +	}
> +	ret = qmc_hdlc_xlate_slot_map(qmc_hdlc, te1->slot_map, &ts_info);
> +	if (ret)
> +		return ret;
> +
> +	ret = qmc_chan_set_ts_info(qmc_hdlc->qmc_chan, &ts_info);
> +	if (ret) {
> +		dev_err(qmc_hdlc->dev, "set QMC channel ts info failed %d\n", ret);
> +		return ret;
> +	}
> +
> +	qmc_hdlc->slot_map = te1->slot_map;
> +
> +	return 0;
> +}
> +
> +static int qmc_hdlc_ioctl(struct net_device *netdev, struct if_settings *ifs)
> +{
> +	struct qmc_hdlc *qmc_hdlc = netdev_to_qmc_hdlc(netdev);
> +	te1_settings te1;
> +
> +	switch (ifs->type) {
> +	case IF_GET_IFACE:
> +		ifs->type = IF_IFACE_E1;
> +		if (ifs->size < sizeof(te1)) {
> +			if (!ifs->size)
> +				return 0; /* only type requested */
> +
> +			ifs->size = sizeof(te1); /* data size wanted */
> +			return -ENOBUFS;
> +		}
> +
> +		memset(&te1, 0, sizeof(te1));
> +
> +		/* Update slot_map */
> +		te1.slot_map = qmc_hdlc->slot_map;
> +
> +		if (copy_to_user(ifs->ifs_ifsu.te1, &te1, sizeof(te1)))
> +			return -EFAULT;
> +		return 0;
> +
> +	case IF_IFACE_E1:
> +	case IF_IFACE_T1:
> +		if (!capable(CAP_NET_ADMIN))
> +			return -EPERM;
> +
> +		if (netdev->flags & IFF_UP)
> +			return -EBUSY;
> +
> +		if (copy_from_user(&te1, ifs->ifs_ifsu.te1, sizeof(te1)))
> +			return -EFAULT;
> +
> +		return qmc_hdlc_set_iface(qmc_hdlc, ifs->type, &te1);
> +
> +	default:
> +		return hdlc_ioctl(netdev, ifs);
> +	}
> +}
> +
>  static int qmc_hdlc_open(struct net_device *netdev)
>  {
>  	struct qmc_hdlc *qmc_hdlc = netdev_to_qmc_hdlc(netdev);
> @@ -326,12 +466,13 @@ static const struct net_device_ops qmc_hdlc_netdev_ops = {
>  	.ndo_open       = qmc_hdlc_open,
>  	.ndo_stop       = qmc_hdlc_close,
>  	.ndo_start_xmit = hdlc_start_xmit,
> -	.ndo_siocwandev	= hdlc_ioctl,
> +	.ndo_siocwandev = qmc_hdlc_ioctl,
>  };
>  
>  static int qmc_hdlc_probe(struct platform_device *pdev)
>  {
>  	struct device *dev = &pdev->dev;
> +	struct qmc_chan_ts_info ts_info;
>  	struct qmc_hdlc *qmc_hdlc;
>  	struct qmc_chan_info info;
>  	hdlc_device *hdlc;
> @@ -357,6 +498,14 @@ static int qmc_hdlc_probe(struct platform_device *pdev)
>  		return dev_err_probe(dev, -EINVAL, "QMC chan mode %d is not QMC_HDLC\n",
>  				     info.mode);
>  
> +	ret = qmc_chan_get_ts_info(qmc_hdlc->qmc_chan, &ts_info);
> +	if (ret)
> +		return dev_err_probe(dev, ret, "get QMC channel ts info failed\n");
> +
> +	ret = qmc_hdlc_xlate_ts_info(qmc_hdlc, &ts_info, &qmc_hdlc->slot_map);
> +	if (ret)
> +		return ret;
> +
>  	qmc_hdlc->netdev = alloc_hdlcdev(qmc_hdlc);
>  	if (!qmc_hdlc->netdev)
>  		return -ENOMEM;
> -- 
> 2.43.0