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