drivers/net/wan/fsl_qmc_hdlc.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Freescale QMC HDLC Device Driver
   4  *
   5  * Copyright 2023 CS GROUP France
   6  *
   7  * Author: Herve Codina <herve.codina@bootlin.com>
   8  */
   9
  10 #include <linux/array_size.h>
  11 #include <linux/bug.h>
  12 #include <linux/cleanup.h>
  13 #include <linux/bitmap.h>
  14 #include <linux/dma-mapping.h>
  15 #include <linux/device.h>
  16 #include <linux/err.h>
  17 #include <linux/framer/framer.h>
  18 #include <linux/hdlc.h>
  19 #include <linux/mod_devicetable.h>
  20 #include <linux/module.h>
  21 #include <linux/platform_device.h>
  22 #include <linux/slab.h>
  23 #include <linux/spinlock.h>
  24 #include <linux/types.h>
  25
  26 #include <soc/fsl/qe/qmc.h>
  27
  28 struct qmc_hdlc_desc {
  29         struct net_device *netdev;
  30         struct sk_buff *skb; /* NULL if the descriptor is not in use */
  31         dma_addr_t dma_addr;
  32         size_t dma_size;
  33 };
  34
  35 struct qmc_hdlc {
  36         struct device *dev;
  37         struct qmc_chan *qmc_chan;
  38         struct net_device *netdev;
  39         struct framer *framer;
  40         spinlock_t carrier_lock; /* Protect carrier detection */
  41         struct notifier_block nb;
  42         bool is_crc32;
  43         spinlock_t tx_lock; /* Protect tx descriptors */
  44         struct qmc_hdlc_desc tx_descs[8];
  45         unsigned int tx_out;
  46         struct qmc_hdlc_desc rx_descs[4];
  47         u32 slot_map;
  48 };
  49
  50 static struct qmc_hdlc *netdev_to_qmc_hdlc(struct net_device *netdev)
  51 {
  52         return dev_to_hdlc(netdev)->priv;
  53 }
  54
  55 static int qmc_hdlc_framer_set_carrier(struct qmc_hdlc *qmc_hdlc)
  56 {
  57         struct framer_status framer_status;
  58         int ret;
  59
  60         if (!qmc_hdlc->framer)
  61                 return 0;
  62
  63         guard(spinlock_irqsave)(&qmc_hdlc->carrier_lock);
  64
  65         ret = framer_get_status(qmc_hdlc->framer, &framer_status);
  66         if (ret) {
  67                 dev_err(qmc_hdlc->dev, "get framer status failed (%d)\n", ret);
  68                 return ret;
  69         }
  70         if (framer_status.link_is_on)
  71                 netif_carrier_on(qmc_hdlc->netdev);
  72         else
  73                 netif_carrier_off(qmc_hdlc->netdev);
  74
  75         return 0;
  76 }
  77
  78 static int qmc_hdlc_framer_notifier(struct notifier_block *nb, unsigned long action,
  79                                     void *data)
  80 {
  81         struct qmc_hdlc *qmc_hdlc = container_of(nb, struct qmc_hdlc, nb);
  82         int ret;
  83
  84         if (action != FRAMER_EVENT_STATUS)
  85                 return NOTIFY_DONE;
  86
  87         ret = qmc_hdlc_framer_set_carrier(qmc_hdlc);
  88         return ret ? NOTIFY_DONE : NOTIFY_OK;
  89 }
  90
  91 static int qmc_hdlc_framer_start(struct qmc_hdlc *qmc_hdlc)
  92 {
  93         struct framer_status framer_status;
  94         int ret;
  95
  96         if (!qmc_hdlc->framer)
  97                 return 0;
  98
  99         ret = framer_power_on(qmc_hdlc->framer);
 100         if (ret) {
 101                 dev_err(qmc_hdlc->dev, "framer power-on failed (%d)\n", ret);
 102                 return ret;
 103         }
 104
 105         /* Be sure that get_status is supported */
 106         ret = framer_get_status(qmc_hdlc->framer, &framer_status);
 107         if (ret) {
 108                 dev_err(qmc_hdlc->dev, "get framer status failed (%d)\n", ret);
 109                 goto framer_power_off;
 110         }
 111
 112         qmc_hdlc->nb.notifier_call = qmc_hdlc_framer_notifier;
 113         ret = framer_notifier_register(qmc_hdlc->framer, &qmc_hdlc->nb);
 114         if (ret) {
 115                 dev_err(qmc_hdlc->dev, "framer notifier register failed (%d)\n", ret);
 116                 goto framer_power_off;
 117         }
 118
 119         return 0;
 120
 121 framer_power_off:
 122         framer_power_off(qmc_hdlc->framer);
 123         return ret;
 124 }
 125
 126 static void qmc_hdlc_framer_stop(struct qmc_hdlc *qmc_hdlc)
 127 {
 128         if (!qmc_hdlc->framer)
 129                 return;
 130
 131         framer_notifier_unregister(qmc_hdlc->framer, &qmc_hdlc->nb);
 132         framer_power_off(qmc_hdlc->framer);
 133 }
 134
 135 static int qmc_hdlc_framer_set_iface(struct qmc_hdlc *qmc_hdlc, int if_iface,
 136                                      const te1_settings *te1)
 137 {
 138         struct framer_config config;
 139         int ret;
 140
 141         if (!qmc_hdlc->framer)
 142                 return 0;
 143
 144         ret = framer_get_config(qmc_hdlc->framer, &config);
 145         if (ret)
 146                 return ret;
 147
 148         switch (if_iface) {
 149         case IF_IFACE_E1:
 150                 config.iface = FRAMER_IFACE_E1;
 151                 break;
 152         case IF_IFACE_T1:
 153                 config.iface = FRAMER_IFACE_T1;
 154                 break;
 155         default:
 156                 return -EINVAL;
 157         }
 158
 159         switch (te1->clock_type) {
 160         case CLOCK_DEFAULT:
 161                 /* Keep current value */
 162                 break;
 163         case CLOCK_EXT:
 164                 config.clock_type = FRAMER_CLOCK_EXT;
 165                 break;
 166         case CLOCK_INT:
 167                 config.clock_type = FRAMER_CLOCK_INT;
 168                 break;
 169         default:
 170                 return -EINVAL;
 171         }
 172         config.line_clock_rate = te1->clock_rate;
 173
 174         return framer_set_config(qmc_hdlc->framer, &config);
 175 }
 176
 177 static int qmc_hdlc_framer_get_iface(struct qmc_hdlc *qmc_hdlc, int *if_iface, te1_settings *te1)
 178 {
 179         struct framer_config config;
 180         int ret;
 181
 182         if (!qmc_hdlc->framer) {
 183                 *if_iface = IF_IFACE_E1;
 184                 return 0;
 185         }
 186
 187         ret = framer_get_config(qmc_hdlc->framer, &config);
 188         if (ret)
 189                 return ret;
 190
 191         switch (config.iface) {
 192         case FRAMER_IFACE_E1:
 193                 *if_iface = IF_IFACE_E1;
 194                 break;
 195         case FRAMER_IFACE_T1:
 196                 *if_iface = IF_IFACE_T1;
 197                 break;
 198         }
 199
 200         if (!te1)
 201                 return 0; /* Only iface type requested */
 202
 203         switch (config.clock_type) {
 204         case FRAMER_CLOCK_EXT:
 205                 te1->clock_type = CLOCK_EXT;
 206                 break;
 207         case FRAMER_CLOCK_INT:
 208                 te1->clock_type = CLOCK_INT;
 209                 break;
 210         default:
 211                 return -EINVAL;
 212         }
 213         te1->clock_rate = config.line_clock_rate;
 214         return 0;
 215 }
 216
 217 static int qmc_hdlc_framer_init(struct qmc_hdlc *qmc_hdlc)
 218 {
 219         int ret;
 220
 221         if (!qmc_hdlc->framer)
 222                 return 0;
 223
 224         ret = framer_init(qmc_hdlc->framer);
 225         if (ret) {
 226                 dev_err(qmc_hdlc->dev, "framer init failed (%d)\n", ret);
 227                 return ret;
 228         }
 229
 230         return 0;
 231 }
 232
 233 static void qmc_hdlc_framer_exit(struct qmc_hdlc *qmc_hdlc)
 234 {
 235         if (!qmc_hdlc->framer)
 236                 return;
 237
 238         framer_exit(qmc_hdlc->framer);
 239 }
 240
 241 static int qmc_hdlc_recv_queue(struct qmc_hdlc *qmc_hdlc, struct qmc_hdlc_desc *desc, size_t size);
 242
 243 #define QMC_HDLC_RX_ERROR_FLAGS                         \
 244         (QMC_RX_FLAG_HDLC_OVF | QMC_RX_FLAG_HDLC_UNA |  \
 245          QMC_RX_FLAG_HDLC_CRC | QMC_RX_FLAG_HDLC_ABORT)
 246
 247 static void qmc_hcld_recv_complete(void *context, size_t length, unsigned int flags)
 248 {
 249         struct qmc_hdlc_desc *desc = context;
 250         struct net_device *netdev;
 251         struct qmc_hdlc *qmc_hdlc;
 252         int ret;
 253
 254         netdev = desc->netdev;
 255         qmc_hdlc = netdev_to_qmc_hdlc(netdev);
 256
 257         dma_unmap_single(qmc_hdlc->dev, desc->dma_addr, desc->dma_size, DMA_FROM_DEVICE);
 258
 259         if (flags & QMC_HDLC_RX_ERROR_FLAGS) {
 260                 netdev->stats.rx_errors++;
 261                 if (flags & QMC_RX_FLAG_HDLC_OVF) /* Data overflow */
 262                         netdev->stats.rx_over_errors++;
 263                 if (flags & QMC_RX_FLAG_HDLC_UNA) /* bits received not multiple of 8 */
 264                         netdev->stats.rx_frame_errors++;
 265                 if (flags & QMC_RX_FLAG_HDLC_ABORT) /* Received an abort sequence */
 266                         netdev->stats.rx_frame_errors++;
 267                 if (flags & QMC_RX_FLAG_HDLC_CRC) /* CRC error */
 268                         netdev->stats.rx_crc_errors++;
 269                 kfree_skb(desc->skb);
 270         } else {
 271                 netdev->stats.rx_packets++;
 272                 netdev->stats.rx_bytes += length;
 273
 274                 skb_put(desc->skb, length);
 275                 desc->skb->protocol = hdlc_type_trans(desc->skb, netdev);
 276                 netif_rx(desc->skb);
 277         }
 278
 279         /* Re-queue a transfer using the same descriptor */
 280         ret = qmc_hdlc_recv_queue(qmc_hdlc, desc, desc->dma_size);
 281         if (ret) {
 282                 dev_err(qmc_hdlc->dev, "queue recv desc failed (%d)\n", ret);
 283                 netdev->stats.rx_errors++;
 284         }
 285 }
 286
 287 static int qmc_hdlc_recv_queue(struct qmc_hdlc *qmc_hdlc, struct qmc_hdlc_desc *desc, size_t size)
 288 {
 289         int ret;
 290
 291         desc->skb = dev_alloc_skb(size);
 292         if (!desc->skb)
 293                 return -ENOMEM;
 294
 295         desc->dma_size = size;
 296         desc->dma_addr = dma_map_single(qmc_hdlc->dev, desc->skb->data,
 297                                         desc->dma_size, DMA_FROM_DEVICE);
 298         ret = dma_mapping_error(qmc_hdlc->dev, desc->dma_addr);
 299         if (ret)
 300                 goto free_skb;
 301
 302         ret = qmc_chan_read_submit(qmc_hdlc->qmc_chan, desc->dma_addr, desc->dma_size,
 303                                    qmc_hcld_recv_complete, desc);
 304         if (ret)
 305                 goto dma_unmap;
 306
 307         return 0;
 308
 309 dma_unmap:
 310         dma_unmap_single(qmc_hdlc->dev, desc->dma_addr, desc->dma_size, DMA_FROM_DEVICE);
 311 free_skb:
 312         kfree_skb(desc->skb);
 313         desc->skb = NULL;
 314         return ret;
 315 }
 316
 317 static void qmc_hdlc_xmit_complete(void *context)
 318 {
 319         struct qmc_hdlc_desc *desc = context;
 320         struct net_device *netdev;
 321         struct qmc_hdlc *qmc_hdlc;
 322         struct sk_buff *skb;
 323
 324         netdev = desc->netdev;
 325         qmc_hdlc = netdev_to_qmc_hdlc(netdev);
 326
 327         scoped_guard(spinlock_irqsave, &qmc_hdlc->tx_lock) {
 328                 dma_unmap_single(qmc_hdlc->dev, desc->dma_addr, desc->dma_size, DMA_TO_DEVICE);
 329                 skb = desc->skb;
 330                 desc->skb = NULL; /* Release the descriptor */
 331                 if (netif_queue_stopped(netdev))
 332                         netif_wake_queue(netdev);
 333         }
 334
 335         netdev->stats.tx_packets++;
 336         netdev->stats.tx_bytes += skb->len;
 337
 338         dev_consume_skb_any(skb);
 339 }
 340
 341 static int qmc_hdlc_xmit_queue(struct qmc_hdlc *qmc_hdlc, struct qmc_hdlc_desc *desc)
 342 {
 343         int ret;
 344
 345         desc->dma_addr = dma_map_single(qmc_hdlc->dev, desc->skb->data,
 346                                         desc->dma_size, DMA_TO_DEVICE);
 347         ret = dma_mapping_error(qmc_hdlc->dev, desc->dma_addr);
 348         if (ret) {
 349                 dev_err(qmc_hdlc->dev, "failed to map skb\n");
 350                 return ret;
 351         }
 352
 353         ret = qmc_chan_write_submit(qmc_hdlc->qmc_chan, desc->dma_addr, desc->dma_size,
 354                                     qmc_hdlc_xmit_complete, desc);
 355         if (ret) {
 356                 dma_unmap_single(qmc_hdlc->dev, desc->dma_addr, desc->dma_size, DMA_TO_DEVICE);
 357                 dev_err(qmc_hdlc->dev, "qmc chan write returns %d\n", ret);
 358                 return ret;
 359         }
 360
 361         return 0;
 362 }
 363
 364 static netdev_tx_t qmc_hdlc_xmit(struct sk_buff *skb, struct net_device *netdev)
 365 {
 366         struct qmc_hdlc *qmc_hdlc = netdev_to_qmc_hdlc(netdev);
 367         struct qmc_hdlc_desc *desc;
 368         int err;
 369
 370         guard(spinlock_irqsave)(&qmc_hdlc->tx_lock);
 371
 372         desc = &qmc_hdlc->tx_descs[qmc_hdlc->tx_out];
 373         if (WARN_ONCE(desc->skb, "No tx descriptors available\n")) {
 374                 /* Should never happen.
 375                  * Previous xmit should have already stopped the queue.
 376                  */
 377                 netif_stop_queue(netdev);
 378                 return NETDEV_TX_BUSY;
 379         }
 380
 381         desc->netdev = netdev;
 382         desc->dma_size = skb->len;
 383         desc->skb = skb;
 384         err = qmc_hdlc_xmit_queue(qmc_hdlc, desc);
 385         if (err) {
 386                 desc->skb = NULL; /* Release the descriptor */
 387                 if (err == -EBUSY) {
 388                         netif_stop_queue(netdev);
 389                         return NETDEV_TX_BUSY;
 390                 }
 391                 dev_kfree_skb(skb);
 392                 netdev->stats.tx_dropped++;
 393                 return NETDEV_TX_OK;
 394         }
 395
 396         qmc_hdlc->tx_out = (qmc_hdlc->tx_out + 1) % ARRAY_SIZE(qmc_hdlc->tx_descs);
 397
 398         if (qmc_hdlc->tx_descs[qmc_hdlc->tx_out].skb)
 399                 netif_stop_queue(netdev);
 400
 401         return NETDEV_TX_OK;
 402 }
 403
 404 static int qmc_hdlc_xlate_slot_map(struct qmc_hdlc *qmc_hdlc,
 405                                    u32 slot_map, struct qmc_chan_ts_info *ts_info)
 406 {
 407         DECLARE_BITMAP(ts_mask_avail, 64);
 408         DECLARE_BITMAP(ts_mask, 64);
 409         DECLARE_BITMAP(map, 64);
 410
 411         /* Tx and Rx available masks must be identical */
 412         if (ts_info->rx_ts_mask_avail != ts_info->tx_ts_mask_avail) {
 413                 dev_err(qmc_hdlc->dev, "tx and rx available timeslots mismatch (0x%llx, 0x%llx)\n",
 414                         ts_info->rx_ts_mask_avail, ts_info->tx_ts_mask_avail);
 415                 return -EINVAL;
 416         }
 417
 418         bitmap_from_u64(ts_mask_avail, ts_info->rx_ts_mask_avail);
 419         bitmap_from_u64(map, slot_map);
 420         bitmap_scatter(ts_mask, map, ts_mask_avail, 64);
 421
 422         if (bitmap_weight(ts_mask, 64) != bitmap_weight(map, 64)) {
 423                 dev_err(qmc_hdlc->dev, "Cannot translate timeslots %64pb -> (%64pb, %64pb)\n",
 424                         map, ts_mask_avail, ts_mask);
 425                 return -EINVAL;
 426         }
 427
 428         bitmap_to_arr64(&ts_info->tx_ts_mask, ts_mask, 64);
 429         ts_info->rx_ts_mask = ts_info->tx_ts_mask;
 430         return 0;
 431 }
 432
 433 static int qmc_hdlc_xlate_ts_info(struct qmc_hdlc *qmc_hdlc,
 434                                   const struct qmc_chan_ts_info *ts_info, u32 *slot_map)
 435 {
 436         DECLARE_BITMAP(ts_mask_avail, 64);
 437         DECLARE_BITMAP(ts_mask, 64);
 438         DECLARE_BITMAP(map, 64);
 439         u32 slot_array[2];
 440
 441         /* Tx and Rx masks and available masks must be identical */
 442         if (ts_info->rx_ts_mask_avail != ts_info->tx_ts_mask_avail) {
 443                 dev_err(qmc_hdlc->dev, "tx and rx available timeslots mismatch (0x%llx, 0x%llx)\n",
 444                         ts_info->rx_ts_mask_avail, ts_info->tx_ts_mask_avail);
 445                 return -EINVAL;
 446         }
 447         if (ts_info->rx_ts_mask != ts_info->tx_ts_mask) {
 448                 dev_err(qmc_hdlc->dev, "tx and rx timeslots mismatch (0x%llx, 0x%llx)\n",
 449                         ts_info->rx_ts_mask, ts_info->tx_ts_mask);
 450                 return -EINVAL;
 451         }
 452
 453         bitmap_from_u64(ts_mask_avail, ts_info->rx_ts_mask_avail);
 454         bitmap_from_u64(ts_mask, ts_info->rx_ts_mask);
 455         bitmap_gather(map, ts_mask, ts_mask_avail, 64);
 456
 457         if (bitmap_weight(ts_mask, 64) != bitmap_weight(map, 64)) {
 458                 dev_err(qmc_hdlc->dev, "Cannot translate timeslots (%64pb, %64pb) -> %64pb\n",
 459                         ts_mask_avail, ts_mask, map);
 460                 return -EINVAL;
 461         }
 462
 463         bitmap_to_arr32(slot_array, map, 64);
 464         if (slot_array[1]) {
 465                 dev_err(qmc_hdlc->dev, "Slot map out of 32bit (%64pb, %64pb) -> %64pb\n",
 466                         ts_mask_avail, ts_mask, map);
 467                 return -EINVAL;
 468         }
 469
 470         *slot_map = slot_array[0];
 471         return 0;
 472 }
 473
 474 static int qmc_hdlc_set_iface(struct qmc_hdlc *qmc_hdlc, int if_iface, const te1_settings *te1)
 475 {
 476         struct qmc_chan_ts_info ts_info;
 477         int ret;
 478
 479         ret = qmc_chan_get_ts_info(qmc_hdlc->qmc_chan, &ts_info);
 480         if (ret) {
 481                 dev_err(qmc_hdlc->dev, "get QMC channel ts info failed %d\n", ret);
 482                 return ret;
 483         }
 484         ret = qmc_hdlc_xlate_slot_map(qmc_hdlc, te1->slot_map, &ts_info);
 485         if (ret)
 486                 return ret;
 487
 488         ret = qmc_chan_set_ts_info(qmc_hdlc->qmc_chan, &ts_info);
 489         if (ret) {
 490                 dev_err(qmc_hdlc->dev, "set QMC channel ts info failed %d\n", ret);
 491                 return ret;
 492         }
 493
 494         qmc_hdlc->slot_map = te1->slot_map;
 495
 496         ret = qmc_hdlc_framer_set_iface(qmc_hdlc, if_iface, te1);
 497         if (ret) {
 498                 dev_err(qmc_hdlc->dev, "framer set iface failed %d\n", ret);
 499                 return ret;
 500         }
 501
 502         return 0;
 503 }
 504
 505 static int qmc_hdlc_ioctl(struct net_device *netdev, struct if_settings *ifs)
 506 {
 507         struct qmc_hdlc *qmc_hdlc = netdev_to_qmc_hdlc(netdev);
 508         te1_settings te1;
 509         int ret;
 510
 511         switch (ifs->type) {
 512         case IF_GET_IFACE:
 513                 if (ifs->size < sizeof(te1)) {
 514                         /* Retrieve type only */
 515                         ret = qmc_hdlc_framer_get_iface(qmc_hdlc, &ifs->type, NULL);
 516                         if (ret)
 517                                 return ret;
 518
 519                         if (!ifs->size)
 520                                 return 0; /* only type requested */
 521
 522                         ifs->size = sizeof(te1); /* data size wanted */
 523                         return -ENOBUFS;
 524                 }
 525
 526                 memset(&te1, 0, sizeof(te1));
 527
 528                 /* Retrieve info from framer */
 529                 ret = qmc_hdlc_framer_get_iface(qmc_hdlc, &ifs->type, &te1);
 530                 if (ret)
 531                         return ret;
 532
 533                 /* Update slot_map */
 534                 te1.slot_map = qmc_hdlc->slot_map;
 535
 536                 if (copy_to_user(ifs->ifs_ifsu.te1, &te1, sizeof(te1)))
 537                         return -EFAULT;
 538                 return 0;
 539
 540         case IF_IFACE_E1:
 541         case IF_IFACE_T1:
 542                 if (!capable(CAP_NET_ADMIN))
 543                         return -EPERM;
 544
 545                 if (netdev->flags & IFF_UP)
 546                         return -EBUSY;
 547
 548                 if (copy_from_user(&te1, ifs->ifs_ifsu.te1, sizeof(te1)))
 549                         return -EFAULT;
 550
 551                 return qmc_hdlc_set_iface(qmc_hdlc, ifs->type, &te1);
 552
 553         default:
 554                 return hdlc_ioctl(netdev, ifs);
 555         }
 556 }
 557
 558 static int qmc_hdlc_open(struct net_device *netdev)
 559 {
 560         struct qmc_hdlc *qmc_hdlc = netdev_to_qmc_hdlc(netdev);
 561         struct qmc_chan_param chan_param;
 562         struct qmc_hdlc_desc *desc;
 563         int ret;
 564         int i;
 565
 566         ret = qmc_hdlc_framer_start(qmc_hdlc);
 567         if (ret)
 568                 return ret;
 569
 570         ret = hdlc_open(netdev);
 571         if (ret)
 572                 goto framer_stop;
 573
 574         /* Update carrier */
 575         qmc_hdlc_framer_set_carrier(qmc_hdlc);
 576
 577         chan_param.mode = QMC_HDLC;
 578         /* HDLC_MAX_MRU + 4 for the CRC
 579          * HDLC_MAX_MRU + 4 + 8 for the CRC and some extraspace needed by the QMC
 580          */
 581         chan_param.hdlc.max_rx_buf_size = HDLC_MAX_MRU + 4 + 8;
 582         chan_param.hdlc.max_rx_frame_size = HDLC_MAX_MRU + 4;
 583         chan_param.hdlc.is_crc32 = qmc_hdlc->is_crc32;
 584         ret = qmc_chan_set_param(qmc_hdlc->qmc_chan, &chan_param);
 585         if (ret) {
 586                 dev_err(qmc_hdlc->dev, "failed to set param (%d)\n", ret);
 587                 goto hdlc_close;
 588         }
 589
 590         /* Queue as many recv descriptors as possible */
 591         for (i = 0; i < ARRAY_SIZE(qmc_hdlc->rx_descs); i++) {
 592                 desc = &qmc_hdlc->rx_descs[i];
 593
 594                 desc->netdev = netdev;
 595                 ret = qmc_hdlc_recv_queue(qmc_hdlc, desc, chan_param.hdlc.max_rx_buf_size);
 596                 if (ret == -EBUSY && i != 0)
 597                         break; /* We use all the QMC chan capability */
 598                 if (ret)
 599                         goto free_desc;
 600         }
 601
 602         ret = qmc_chan_start(qmc_hdlc->qmc_chan, QMC_CHAN_ALL);
 603         if (ret) {
 604                 dev_err(qmc_hdlc->dev, "qmc chan start failed (%d)\n", ret);
 605                 goto free_desc;
 606         }
 607
 608         netif_start_queue(netdev);
 609
 610         return 0;
 611
 612 free_desc:
 613         qmc_chan_reset(qmc_hdlc->qmc_chan, QMC_CHAN_ALL);
 614         while (i--) {
 615                 desc = &qmc_hdlc->rx_descs[i];
 616                 dma_unmap_single(qmc_hdlc->dev, desc->dma_addr, desc->dma_size,
 617                                  DMA_FROM_DEVICE);
 618                 kfree_skb(desc->skb);
 619                 desc->skb = NULL;
 620         }
 621 hdlc_close:
 622         hdlc_close(netdev);
 623 framer_stop:
 624         qmc_hdlc_framer_stop(qmc_hdlc);
 625         return ret;
 626 }
 627
 628 static int qmc_hdlc_close(struct net_device *netdev)
 629 {
 630         struct qmc_hdlc *qmc_hdlc = netdev_to_qmc_hdlc(netdev);
 631         struct qmc_hdlc_desc *desc;
 632         int i;
 633
 634         qmc_chan_stop(qmc_hdlc->qmc_chan, QMC_CHAN_ALL);
 635         qmc_chan_reset(qmc_hdlc->qmc_chan, QMC_CHAN_ALL);
 636
 637         netif_stop_queue(netdev);
 638
 639         for (i = 0; i < ARRAY_SIZE(qmc_hdlc->tx_descs); i++) {
 640                 desc = &qmc_hdlc->tx_descs[i];
 641                 if (!desc->skb)
 642                         continue;
 643                 dma_unmap_single(qmc_hdlc->dev, desc->dma_addr, desc->dma_size,
 644                                  DMA_TO_DEVICE);
 645                 kfree_skb(desc->skb);
 646                 desc->skb = NULL;
 647         }
 648
 649         for (i = 0; i < ARRAY_SIZE(qmc_hdlc->rx_descs); i++) {
 650                 desc = &qmc_hdlc->rx_descs[i];
 651                 if (!desc->skb)
 652                         continue;
 653                 dma_unmap_single(qmc_hdlc->dev, desc->dma_addr, desc->dma_size,
 654                                  DMA_FROM_DEVICE);
 655                 kfree_skb(desc->skb);
 656                 desc->skb = NULL;
 657         }
 658
 659         hdlc_close(netdev);
 660         qmc_hdlc_framer_stop(qmc_hdlc);
 661         return 0;
 662 }
 663
 664 static int qmc_hdlc_attach(struct net_device *netdev, unsigned short encoding,
 665                            unsigned short parity)
 666 {
 667         struct qmc_hdlc *qmc_hdlc = netdev_to_qmc_hdlc(netdev);
 668
 669         if (encoding != ENCODING_NRZ)
 670                 return -EINVAL;
 671
 672         switch (parity) {
 673         case PARITY_CRC16_PR1_CCITT:
 674                 qmc_hdlc->is_crc32 = false;
 675                 break;
 676         case PARITY_CRC32_PR1_CCITT:
 677                 qmc_hdlc->is_crc32 = true;
 678                 break;
 679         default:
 680                 dev_err(qmc_hdlc->dev, "unsupported parity %u\n", parity);
 681                 return -EINVAL;
 682         }
 683
 684         return 0;
 685 }
 686
 687 static const struct net_device_ops qmc_hdlc_netdev_ops = {
 688         .ndo_open       = qmc_hdlc_open,
 689         .ndo_stop       = qmc_hdlc_close,
 690         .ndo_start_xmit = hdlc_start_xmit,
 691         .ndo_siocwandev = qmc_hdlc_ioctl,
 692 };
 693
 694 static int qmc_hdlc_probe(struct platform_device *pdev)
 695 {
 696         struct device *dev = &pdev->dev;
 697         struct qmc_chan_ts_info ts_info;
 698         struct qmc_hdlc *qmc_hdlc;
 699         struct qmc_chan_info info;
 700         hdlc_device *hdlc;
 701         int ret;
 702
 703         qmc_hdlc = devm_kzalloc(dev, sizeof(*qmc_hdlc), GFP_KERNEL);
 704         if (!qmc_hdlc)
 705                 return -ENOMEM;
 706
 707         qmc_hdlc->dev = dev;
 708         spin_lock_init(&qmc_hdlc->tx_lock);
 709         spin_lock_init(&qmc_hdlc->carrier_lock);
 710
 711         qmc_hdlc->qmc_chan = devm_qmc_chan_get_bychild(dev, dev->of_node);
 712         if (IS_ERR(qmc_hdlc->qmc_chan))
 713                 return dev_err_probe(dev, PTR_ERR(qmc_hdlc->qmc_chan),
 714                                      "get QMC channel failed\n");
 715
 716         ret = qmc_chan_get_info(qmc_hdlc->qmc_chan, &info);
 717         if (ret)
 718                 return dev_err_probe(dev, ret, "get QMC channel info failed\n");
 719
 720         if (info.mode != QMC_HDLC)
 721                 return dev_err_probe(dev, -EINVAL, "QMC chan mode %d is not QMC_HDLC\n",
 722                                      info.mode);
 723
 724         ret = qmc_chan_get_ts_info(qmc_hdlc->qmc_chan, &ts_info);
 725         if (ret)
 726                 return dev_err_probe(dev, ret, "get QMC channel ts info failed\n");
 727
 728         ret = qmc_hdlc_xlate_ts_info(qmc_hdlc, &ts_info, &qmc_hdlc->slot_map);
 729         if (ret)
 730                 return ret;
 731
 732         qmc_hdlc->framer = devm_framer_optional_get(dev, "fsl,framer");
 733         if (IS_ERR(qmc_hdlc->framer))
 734                 return PTR_ERR(qmc_hdlc->framer);
 735
 736         ret = qmc_hdlc_framer_init(qmc_hdlc);
 737         if (ret)
 738                 return ret;
 739
 740         qmc_hdlc->netdev = alloc_hdlcdev(qmc_hdlc);
 741         if (!qmc_hdlc->netdev) {
 742                 ret = -ENOMEM;
 743                 goto framer_exit;
 744         }
 745
 746         hdlc = dev_to_hdlc(qmc_hdlc->netdev);
 747         hdlc->attach = qmc_hdlc_attach;
 748         hdlc->xmit = qmc_hdlc_xmit;
 749         SET_NETDEV_DEV(qmc_hdlc->netdev, dev);
 750         qmc_hdlc->netdev->tx_queue_len = ARRAY_SIZE(qmc_hdlc->tx_descs);
 751         qmc_hdlc->netdev->netdev_ops = &qmc_hdlc_netdev_ops;
 752         ret = register_hdlc_device(qmc_hdlc->netdev);
 753         if (ret) {
 754                 dev_err_probe(dev, ret, "failed to register hdlc device\n");
 755                 goto free_netdev;
 756         }
 757
 758         platform_set_drvdata(pdev, qmc_hdlc);
 759         return 0;
 760
 761 free_netdev:
 762         free_netdev(qmc_hdlc->netdev);
 763 framer_exit:
 764         qmc_hdlc_framer_exit(qmc_hdlc);
 765         return ret;
 766 }
 767
 768 static int qmc_hdlc_remove(struct platform_device *pdev)
 769 {
 770         struct qmc_hdlc *qmc_hdlc = platform_get_drvdata(pdev);
 771
 772         unregister_hdlc_device(qmc_hdlc->netdev);
 773         free_netdev(qmc_hdlc->netdev);
 774         qmc_hdlc_framer_exit(qmc_hdlc);
 775
 776         return 0;
 777 }
 778
 779 static const struct of_device_id qmc_hdlc_id_table[] = {
 780         { .compatible = "fsl,qmc-hdlc" },
 781         {} /* sentinel */
 782 };
 783 MODULE_DEVICE_TABLE(of, qmc_hdlc_id_table);
 784
 785 static struct platform_driver qmc_hdlc_driver = {
 786         .driver = {
 787                 .name = "fsl-qmc-hdlc",
 788                 .of_match_table = qmc_hdlc_id_table,
 789         },
 790         .probe = qmc_hdlc_probe,
 791         .remove = qmc_hdlc_remove,
 792 };
 793 module_platform_driver(qmc_hdlc_driver);
 794
 795 MODULE_AUTHOR("Herve Codina <herve.codina@bootlin.com>");
 796 MODULE_DESCRIPTION("QMC HDLC driver");
 797 MODULE_LICENSE("GPL");