target/linux/layerscape/patches-5.4/701-net-0273-net-dsa-ocelot-add-tsn-support-for-felix-switch.patch

   1 From 36b446d44d66a6d6a072d3f5e87ebb05e0b88d98 Mon Sep 17 00:00:00 2001
   2 From: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
   3 Date: Fri, 29 Nov 2019 14:28:37 +0800
   4 Subject: [PATCH] net: dsa: ocelot: add tsn support for felix switch
   5
   6 Support tsn capabilities in DSA felix switch driver. This felix tsn
   7 driver is using tsn configuration of ocelot, and registered on each
   8 switch port through DSA port setup.
   9
  10 Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
  11 ---
  12  drivers/net/dsa/ocelot/Kconfig         |   8 +
  13  drivers/net/dsa/ocelot/Makefile        |   2 +
  14  drivers/net/dsa/ocelot/felix.c         |  50 ++++
  15  drivers/net/dsa/ocelot/felix_tsn.c     | 432 +++++++++++++++++++++++++++++++++
  16  drivers/net/dsa/ocelot/felix_tsn.h     |  61 +++++
  17  drivers/net/dsa/ocelot/felix_vsc9959.c |   8 +-
  18  include/net/dsa.h                      |   1 +
  19  net/dsa/dsa2.c                         |   4 +
  20  8 files changed, 564 insertions(+), 2 deletions(-)
  21  create mode 100644 drivers/net/dsa/ocelot/felix_tsn.c
  22  create mode 100644 drivers/net/dsa/ocelot/felix_tsn.h
  23
  24 --- a/drivers/net/dsa/ocelot/Kconfig
  25 +++ b/drivers/net/dsa/ocelot/Kconfig
  26 @@ -9,3 +9,11 @@ config NET_DSA_MSCC_FELIX
  27           the Vitesse / Microsemi / Microchip Ocelot family of switching cores.
  28           It is embedded as a PCIe function of the NXP LS1028A ENETC integrated
  29           endpoint.
  30 +
  31 +config MSCC_FELIX_SWITCH_TSN
  32 +       tristate "TSN on FELIX switch driver"
  33 +       depends on NET_DSA_MSCC_FELIX
  34 +       depends on TSN
  35 +       help
  36 +         This driver supports TSN on felix switch.
  37 +
  38 --- a/drivers/net/dsa/ocelot/Makefile
  39 +++ b/drivers/net/dsa/ocelot/Makefile
  40 @@ -4,3 +4,5 @@ obj-$(CONFIG_NET_DSA_MSCC_FELIX) += mscc
  41  mscc_felix-objs := \
  42         felix.o \
  43         felix_vsc9959.o
  44 +
  45 +obj-$(CONFIG_MSCC_FELIX_SWITCH_TSN) += felix_tsn.o
  46 --- a/drivers/net/dsa/ocelot/felix.c
  47 +++ b/drivers/net/dsa/ocelot/felix.c
  48 @@ -9,6 +9,38 @@
  49  #include <linux/of.h>
  50  #include <net/dsa.h>
  51  #include "felix.h"
  52 +#include "felix_tsn.h"
  53 +
  54 +#ifdef CONFIG_MSCC_FELIX_SWITCH_TSN
  55 +const struct tsn_ops switch_tsn_ops = {
  56 +       .device_init                    = felix_tsn_init,
  57 +       .get_capability                 = felix_tsn_get_cap,
  58 +       .qbv_set                        = felix_qbv_set,
  59 +       .qbv_get                        = felix_qbv_get,
  60 +       .qbv_get_status                 = felix_qbv_get_status,
  61 +       .qbu_set                        = felix_qbu_set,
  62 +       .qbu_get                        = felix_qbu_get,
  63 +       .cb_streamid_set                = felix_cb_streamid_set,
  64 +       .cb_streamid_get                = felix_cb_streamid_get,
  65 +       .cb_streamid_counters_get       = felix_cb_streamid_counters_get,
  66 +       .qci_sfi_set                    = felix_qci_sfi_set,
  67 +       .qci_sfi_get                    = felix_qci_sfi_get,
  68 +       .qci_sfi_counters_get           = felix_qci_sfi_counters_get,
  69 +       .qci_get_maxcap                 = felix_qci_max_cap_get,
  70 +       .qci_sgi_set                    = felix_qci_sgi_set,
  71 +       .qci_sgi_get                    = felix_qci_sgi_get,
  72 +       .qci_sgi_status_get             = felix_qci_sgi_status_get,
  73 +       .qci_fmi_set                    = felix_qci_fmi_set,
  74 +       .qci_fmi_get                    = felix_qci_fmi_get,
  75 +       .cbs_set                        = felix_cbs_set,
  76 +       .cbs_get                        = felix_cbs_get,
  77 +       .ct_set                         = felix_cut_thru_set,
  78 +       .cbgen_set                      = felix_seq_gen_set,
  79 +       .cbrec_set                      = felix_seq_rec_set,
  80 +       .cb_get                         = felix_cb_get,
  81 +       .dscp_set                       = felix_dscp_set,
  82 +};
  83 +#endif
  84
  85  static enum dsa_tag_protocol felix_get_tag_protocol(struct dsa_switch *ds,
  86                                                     int port)
  87 @@ -138,6 +170,21 @@ static int felix_vlan_del(struct dsa_swi
  88         return 0;
  89  }
  90
  91 +#ifdef CONFIG_MSCC_FELIX_SWITCH_TSN
  92 +static int felix_tsn_enable(struct dsa_port *dp)
  93 +{
  94 +       struct net_device *dev;
  95 +
  96 +       if (dp->type == DSA_PORT_TYPE_USER) {
  97 +               dev = dp->slave;
  98 +               tsn_port_register(dev,
  99 +                                 (struct tsn_ops *)&switch_tsn_ops,
 100 +                                 GROUP_OFFSET_SWITCH);
 101 +       }
 102 +       return 0;
 103 +}
 104 +#endif
 105 +
 106  static int felix_port_enable(struct dsa_switch *ds, int port,
 107                              struct phy_device *phy)
 108  {
 109 @@ -386,6 +433,9 @@ static const struct dsa_switch_ops felix
 110         .port_hwtstamp_set      = felix_hwtstamp_set,
 111         .port_rxtstamp          = felix_rxtstamp,
 112         .port_txtstamp          = felix_txtstamp,
 113 +#ifdef CONFIG_MSCC_FELIX_SWITCH_TSN
 114 +       .port_tsn_enable        = felix_tsn_enable,
 115 +#endif
 116  };
 117
 118  static struct felix_info *felix_instance_tbl[] = {
 119 --- /dev/null
 120 +++ b/drivers/net/dsa/ocelot/felix_tsn.c
 121 @@ -0,0 +1,432 @@
 122 +// SPDX-License-Identifier: (GPL-2.0 OR MIT)
 123 +/* Felix Switch TSN driver
 124 + *
 125 + * Copyright 2018-2019 NXP
 126 + */
 127 +
 128 +#include <linux/io.h>
 129 +#include <linux/module.h>
 130 +#include <linux/kernel.h>
 131 +#include <linux/pci.h>
 132 +#include <soc/mscc/ocelot.h>
 133 +#include <net/tsn.h>
 134 +#include "felix.h"
 135 +
 136 +static struct ocelot *felix_dev_to_ocelot(struct net_device *ndev)
 137 +{
 138 +       struct pci_dev *pdev;
 139 +       struct felix *felix;
 140 +
 141 +       pdev = list_entry(ndev->dev.parent, struct pci_dev, dev);
 142 +       felix = pci_get_drvdata(pdev);
 143 +       if (!felix)
 144 +               return NULL;
 145 +
 146 +       return &felix->ocelot;
 147 +}
 148 +
 149 +static int felix_dev_to_port(struct net_device *ndev, struct ocelot *ocelot)
 150 +{
 151 +       struct felix *felix = ocelot_to_felix(ocelot);
 152 +       struct dsa_switch *ds = felix->ds;
 153 +       struct dsa_port *dp;
 154 +       int i;
 155 +
 156 +       for (i = 0; i < ds->num_ports; i++) {
 157 +               dp = &ds->ports[i];
 158 +               if (dp->dn == ndev->dev.of_node)
 159 +                       return dp->index;
 160 +       }
 161 +
 162 +       return -ENODEV;
 163 +}
 164 +
 165 +u32 felix_tsn_get_cap(struct net_device *ndev)
 166 +{
 167 +       u32 cap = 0;
 168 +
 169 +       cap = (TSN_CAP_QBV | TSN_CAP_QCI | TSN_CAP_QBU | TSN_CAP_CBS |
 170 +              TSN_CAP_CB | TSN_CAP_TBS | TSN_CAP_CTH);
 171 +
 172 +       return cap;
 173 +}
 174 +
 175 +int felix_qbv_set(struct net_device *ndev,
 176 +                 struct tsn_qbv_conf *shaper_config)
 177 +{
 178 +       struct ocelot *ocelot;
 179 +       int port;
 180 +
 181 +       ocelot = felix_dev_to_ocelot(ndev);
 182 +       if (!ocelot)
 183 +               return -ENODEV;
 184 +       port = felix_dev_to_port(ndev, ocelot);
 185 +       if (port < 0)
 186 +               return -ENODEV;
 187 +
 188 +       return ocelot_qbv_set(ocelot, port, shaper_config);
 189 +}
 190 +
 191 +int felix_qbv_get(struct net_device *ndev,
 192 +                 struct tsn_qbv_conf *shaper_config)
 193 +{
 194 +       struct ocelot *ocelot;
 195 +       int port;
 196 +
 197 +       ocelot = felix_dev_to_ocelot(ndev);
 198 +       if (!ocelot)
 199 +               return -ENODEV;
 200 +       port = felix_dev_to_port(ndev, ocelot);
 201 +       if (port < 0)
 202 +               return -ENODEV;
 203 +
 204 +       return ocelot_qbv_get(ocelot, port, shaper_config);
 205 +}
 206 +
 207 +int felix_qbv_get_status(struct net_device *ndev,
 208 +                        struct tsn_qbv_status *qbvstatus)
 209 +{
 210 +       struct ocelot *ocelot;
 211 +       int port;
 212 +
 213 +       ocelot = felix_dev_to_ocelot(ndev);
 214 +       if (!ocelot)
 215 +               return -ENODEV;
 216 +       port = felix_dev_to_port(ndev, ocelot);
 217 +       if (port < 0)
 218 +               return -ENODEV;
 219 +
 220 +       return ocelot_qbv_get_status(ocelot, port, qbvstatus);
 221 +}
 222 +
 223 +int felix_qbu_set(struct net_device *ndev, u8 preemptible)
 224 +{
 225 +       struct ocelot *ocelot;
 226 +       int port;
 227 +
 228 +       ocelot = felix_dev_to_ocelot(ndev);
 229 +       if (!ocelot)
 230 +               return -ENODEV;
 231 +       port = felix_dev_to_port(ndev, ocelot);
 232 +       if (port < 0)
 233 +               return -ENODEV;
 234 +
 235 +       return ocelot_qbu_set(ocelot, port, preemptible);
 236 +}
 237 +
 238 +int felix_qbu_get(struct net_device *ndev, struct tsn_preempt_status *c)
 239 +{
 240 +       struct ocelot *ocelot;
 241 +       int port;
 242 +
 243 +       ocelot = felix_dev_to_ocelot(ndev);
 244 +       if (!ocelot)
 245 +               return -ENODEV;
 246 +       port = felix_dev_to_port(ndev, ocelot);
 247 +       if (port < 0)
 248 +               return -ENODEV;
 249 +
 250 +       return ocelot_qbu_get(ocelot, port, c);
 251 +}
 252 +
 253 +int felix_cb_streamid_set(struct net_device *ndev, u32 index, bool enable,
 254 +                         struct tsn_cb_streamid *streamid)
 255 +{
 256 +       struct ocelot *ocelot;
 257 +       int port;
 258 +
 259 +       ocelot = felix_dev_to_ocelot(ndev);
 260 +       if (!ocelot)
 261 +               return -ENODEV;
 262 +       port = felix_dev_to_port(ndev, ocelot);
 263 +       if (port < 0)
 264 +               return -ENODEV;
 265 +
 266 +       return ocelot_cb_streamid_set(ocelot, port, index, enable, streamid);
 267 +}
 268 +
 269 +int felix_cb_streamid_get(struct net_device *ndev, u32 index,
 270 +                         struct tsn_cb_streamid *streamid)
 271 +{
 272 +       struct ocelot *ocelot;
 273 +       int port;
 274 +
 275 +       ocelot = felix_dev_to_ocelot(ndev);
 276 +       if (!ocelot)
 277 +               return -ENODEV;
 278 +       port = felix_dev_to_port(ndev, ocelot);
 279 +       if (port < 0)
 280 +               return -ENODEV;
 281 +
 282 +       return ocelot_cb_streamid_get(ocelot, port, index, streamid);
 283 +}
 284 +
 285 +int felix_cb_streamid_counters_get(struct net_device *ndev, u32 index,
 286 +                                  struct tsn_cb_streamid_counters *sc)
 287 +{
 288 +       return 0;
 289 +}
 290 +
 291 +int felix_qci_sfi_set(struct net_device *ndev, u32 index, bool enable,
 292 +                     struct tsn_qci_psfp_sfi_conf *sfi)
 293 +{
 294 +       struct ocelot *ocelot;
 295 +       int port;
 296 +
 297 +       ocelot = felix_dev_to_ocelot(ndev);
 298 +       if (!ocelot)
 299 +               return -ENODEV;
 300 +       port = felix_dev_to_port(ndev, ocelot);
 301 +       if (port < 0)
 302 +               return -ENODEV;
 303 +
 304 +       return ocelot_qci_sfi_set(ocelot, port, index, enable, sfi);
 305 +}
 306 +
 307 +int felix_qci_sfi_get(struct net_device *ndev, u32 index,
 308 +                     struct tsn_qci_psfp_sfi_conf *sfi)
 309 +{
 310 +       struct ocelot *ocelot;
 311 +       int port;
 312 +
 313 +       ocelot = felix_dev_to_ocelot(ndev);
 314 +       if (!ocelot)
 315 +               return -ENODEV;
 316 +       port = felix_dev_to_port(ndev, ocelot);
 317 +       if (port < 0)
 318 +               return -ENODEV;
 319 +
 320 +       return ocelot_qci_sfi_get(ocelot, port, index, sfi);
 321 +}
 322 +
 323 +int felix_qci_sfi_counters_get(struct net_device *ndev, u32 index,
 324 +                              struct tsn_qci_psfp_sfi_counters *sfi_cnt)
 325 +{
 326 +       struct ocelot *ocelot;
 327 +       int port;
 328 +
 329 +       ocelot = felix_dev_to_ocelot(ndev);
 330 +       if (!ocelot)
 331 +               return -ENODEV;
 332 +       port = felix_dev_to_port(ndev, ocelot);
 333 +       if (port < 0)
 334 +               return -ENODEV;
 335 +
 336 +       return ocelot_qci_sfi_counters_get(ocelot, port, index, sfi_cnt);
 337 +}
 338 +
 339 +int felix_qci_max_cap_get(struct net_device *ndev,
 340 +                         struct tsn_qci_psfp_stream_param *stream_para)
 341 +{
 342 +       struct ocelot *ocelot;
 343 +
 344 +       ocelot = felix_dev_to_ocelot(ndev);
 345 +       if (!ocelot)
 346 +               return -ENODEV;
 347 +
 348 +       return ocelot_qci_max_cap_get(ocelot, stream_para);
 349 +}
 350 +
 351 +int felix_qci_sgi_set(struct net_device *ndev, u32 index,
 352 +                     struct tsn_qci_psfp_sgi_conf *sgi_conf)
 353 +{
 354 +       struct ocelot *ocelot;
 355 +       int port;
 356 +
 357 +       ocelot = felix_dev_to_ocelot(ndev);
 358 +       if (!ocelot)
 359 +               return -ENODEV;
 360 +       port = felix_dev_to_port(ndev, ocelot);
 361 +       if (port < 0)
 362 +               return -ENODEV;
 363 +
 364 +       return ocelot_qci_sgi_set(ocelot, port, index, sgi_conf);
 365 +}
 366 +
 367 +int felix_qci_sgi_get(struct net_device *ndev, u32 index,
 368 +                     struct tsn_qci_psfp_sgi_conf *sgi_conf)
 369 +{
 370 +       struct ocelot *ocelot;
 371 +       int port;
 372 +
 373 +       ocelot = felix_dev_to_ocelot(ndev);
 374 +       if (!ocelot)
 375 +               return -ENODEV;
 376 +       port = felix_dev_to_port(ndev, ocelot);
 377 +       if (port < 0)
 378 +               return -ENODEV;
 379 +
 380 +       return ocelot_qci_sgi_get(ocelot, port, index, sgi_conf);
 381 +}
 382 +
 383 +int felix_qci_sgi_status_get(struct net_device *ndev, u32 index,
 384 +                            struct tsn_psfp_sgi_status *sgi_status)
 385 +{
 386 +       struct ocelot *ocelot;
 387 +       int port;
 388 +
 389 +       ocelot = felix_dev_to_ocelot(ndev);
 390 +       if (!ocelot)
 391 +               return -ENODEV;
 392 +       port = felix_dev_to_port(ndev, ocelot);
 393 +       if (port < 0)
 394 +               return -ENODEV;
 395 +
 396 +       return ocelot_qci_sgi_status_get(ocelot, port, index, sgi_status);
 397 +}
 398 +
 399 +int felix_qci_fmi_set(struct net_device *ndev, u32 index,
 400 +                     bool enable, struct tsn_qci_psfp_fmi *fmi)
 401 +{
 402 +       struct ocelot *ocelot;
 403 +       int port;
 404 +
 405 +       ocelot = felix_dev_to_ocelot(ndev);
 406 +       if (!ocelot)
 407 +               return -ENODEV;
 408 +       port = felix_dev_to_port(ndev, ocelot);
 409 +       if (port < 0)
 410 +               return -ENODEV;
 411 +
 412 +       return ocelot_qci_fmi_set(ocelot, port, index, enable, fmi);
 413 +}
 414 +
 415 +int felix_qci_fmi_get(struct net_device *ndev, u32 index,
 416 +                     struct tsn_qci_psfp_fmi *fmi,
 417 +                     struct tsn_qci_psfp_fmi_counters *counters)
 418 +{
 419 +       struct ocelot *ocelot;
 420 +       int port;
 421 +
 422 +       ocelot = felix_dev_to_ocelot(ndev);
 423 +       if (!ocelot)
 424 +               return -ENODEV;
 425 +       port = felix_dev_to_port(ndev, ocelot);
 426 +       if (port < 0)
 427 +               return -ENODEV;
 428 +
 429 +       return ocelot_qci_fmi_get(ocelot, port, index, fmi, counters);
 430 +}
 431 +
 432 +int felix_cbs_set(struct net_device *ndev, u8 tc, u8 bw)
 433 +{
 434 +       struct ocelot *ocelot;
 435 +       int port;
 436 +
 437 +       ocelot = felix_dev_to_ocelot(ndev);
 438 +       if (!ocelot)
 439 +               return -ENODEV;
 440 +       port = felix_dev_to_port(ndev, ocelot);
 441 +       if (port < 0)
 442 +               return -ENODEV;
 443 +
 444 +       return ocelot_cbs_set(ocelot, port, tc, bw);
 445 +}
 446 +
 447 +int felix_cbs_get(struct net_device *ndev, u8 tc)
 448 +{
 449 +       struct ocelot *ocelot;
 450 +       int port;
 451 +
 452 +       ocelot = felix_dev_to_ocelot(ndev);
 453 +       if (!ocelot)
 454 +               return -ENODEV;
 455 +       port = felix_dev_to_port(ndev, ocelot);
 456 +       if (port < 0)
 457 +               return -ENODEV;
 458 +
 459 +       return ocelot_cbs_get(ocelot, port, tc);
 460 +}
 461 +
 462 +int felix_cut_thru_set(struct net_device *ndev, u8 cut_thru)
 463 +{
 464 +       struct ocelot *ocelot;
 465 +       int port;
 466 +
 467 +       ocelot = felix_dev_to_ocelot(ndev);
 468 +       if (!ocelot)
 469 +               return -ENODEV;
 470 +       port = felix_dev_to_port(ndev, ocelot);
 471 +       if (port < 0)
 472 +               return -ENODEV;
 473 +
 474 +       return ocelot_cut_thru_set(ocelot, port, cut_thru);
 475 +}
 476 +
 477 +int felix_seq_gen_set(struct net_device *ndev, u32 index,
 478 +                     struct tsn_seq_gen_conf *sg_conf)
 479 +{
 480 +       struct ocelot *ocelot;
 481 +       int port;
 482 +
 483 +       ocelot = felix_dev_to_ocelot(ndev);
 484 +       if (!ocelot)
 485 +               return -ENODEV;
 486 +       port = felix_dev_to_port(ndev, ocelot);
 487 +       if (port < 0)
 488 +               return -ENODEV;
 489 +
 490 +       return ocelot_seq_gen_set(ocelot, port, index, sg_conf);
 491 +}
 492 +
 493 +int felix_seq_rec_set(struct net_device *ndev, u32 index,
 494 +                     struct tsn_seq_rec_conf *sr_conf)
 495 +{
 496 +       struct ocelot *ocelot;
 497 +       int port;
 498 +
 499 +       ocelot = felix_dev_to_ocelot(ndev);
 500 +       if (!ocelot)
 501 +               return -ENODEV;
 502 +       port = felix_dev_to_port(ndev, ocelot);
 503 +       if (port < 0)
 504 +               return -ENODEV;
 505 +
 506 +       return ocelot_seq_rec_set(ocelot, port, index, sr_conf);
 507 +}
 508 +
 509 +int felix_cb_get(struct net_device *ndev, u32 index,
 510 +                struct tsn_cb_status *c)
 511 +{
 512 +       struct ocelot *ocelot;
 513 +       int port;
 514 +
 515 +       ocelot = felix_dev_to_ocelot(ndev);
 516 +       if (!ocelot)
 517 +               return -ENODEV;
 518 +       port = felix_dev_to_port(ndev, ocelot);
 519 +       if (port < 0)
 520 +               return -ENODEV;
 521 +
 522 +       return ocelot_cb_get(ocelot, port, index, c);
 523 +}
 524 +
 525 +int felix_dscp_set(struct net_device *ndev, bool enable, const u8 dscp_ix,
 526 +                  struct tsn_qos_switch_dscp_conf *c)
 527 +{
 528 +       struct ocelot *ocelot;
 529 +       int port;
 530 +
 531 +       ocelot = felix_dev_to_ocelot(ndev);
 532 +       if (!ocelot)
 533 +               return -ENODEV;
 534 +       port = felix_dev_to_port(ndev, ocelot);
 535 +       if (port < 0)
 536 +               return -ENODEV;
 537 +
 538 +       return ocelot_dscp_set(ocelot, port, enable, dscp_ix, c);
 539 +}
 540 +
 541 +void felix_tsn_init(struct net_device *ndev)
 542 +{
 543 +       struct ocelot *ocelot;
 544 +       int port;
 545 +
 546 +       ocelot = felix_dev_to_ocelot(ndev);
 547 +       if (!ocelot)
 548 +               return;
 549 +       port = felix_dev_to_port(ndev, ocelot);
 550 +
 551 +       ocelot_pcp_map_enable(ocelot, port);
 552 +       ocelot_rtag_parse_enable(ocelot, port);
 553 +}
 554 --- /dev/null
 555 +++ b/drivers/net/dsa/ocelot/felix_tsn.h
 556 @@ -0,0 +1,61 @@
 557 +/* SPDX-License-Identifier: (GPL-2.0 OR MIT)
 558 + *
 559 + * TSN_SWITCH driver
 560 + *
 561 + * Copyright 2018-2019 NXP
 562 + */
 563 +
 564 +#ifndef _MSCC_FELIX_SWITCH_TSN_H_
 565 +#define _MSCC_FELIX_SWITCH_TSN_H_
 566 +#include <net/tsn.h>
 567 +
 568 +u32 felix_tsn_get_cap(struct net_device *ndev);
 569 +int felix_qbv_set(struct net_device *ndev,
 570 +                 struct tsn_qbv_conf *shaper_config);
 571 +int felix_qbv_get(struct net_device *ndev,
 572 +                 struct tsn_qbv_conf *shaper_config);
 573 +int felix_qbv_get_status(struct net_device *ndev,
 574 +                        struct tsn_qbv_status *qbvstatus);
 575 +int felix_cut_thru_set(struct net_device *ndev, u8 cut_thru);
 576 +int felix_cbs_set(struct net_device *ndev, u8 tc, u8 bw);
 577 +int felix_cbs_get(struct net_device *ndev, u8 tc);
 578 +int felix_qbu_set(struct net_device *ndev, u8 preemptible);
 579 +int felix_qbu_get(struct net_device *ndev, struct tsn_preempt_status *c);
 580 +int felix_cb_streamid_get(struct net_device *ndev, u32 index,
 581 +                         struct tsn_cb_streamid *streamid);
 582 +int felix_cb_streamid_set(struct net_device *ndev, u32 index,
 583 +                         bool enable, struct tsn_cb_streamid *streamid);
 584 +int felix_cb_streamid_counters_get(struct net_device *ndev, u32 index,
 585 +                                  struct tsn_cb_streamid_counters *sc);
 586 +int felix_qci_sfi_get(struct net_device *ndev, u32 index,
 587 +                     struct tsn_qci_psfp_sfi_conf *sfi);
 588 +int felix_qci_sfi_set(struct net_device *ndev, u32 index,
 589 +                     bool enable, struct tsn_qci_psfp_sfi_conf *sfi);
 590 +int felix_cb_streamid_counters_get(struct net_device *ndev, u32 index,
 591 +                                  struct tsn_cb_streamid_counters *s_counters);
 592 +int felix_qci_sfi_counters_get(struct net_device *ndev, u32 index,
 593 +                              struct tsn_qci_psfp_sfi_counters *sfi_counters);
 594 +int felix_qci_max_cap_get(struct net_device *ndev,
 595 +                         struct tsn_qci_psfp_stream_param *stream_para);
 596 +int felix_qci_sgi_set(struct net_device *ndev, u32 index,
 597 +                     struct tsn_qci_psfp_sgi_conf *sgi_conf);
 598 +int felix_qci_sgi_get(struct net_device *ndev, u32 index,
 599 +                     struct tsn_qci_psfp_sgi_conf *sgi_conf);
 600 +int felix_qci_sgi_status_get(struct net_device *ndev, u16 index,
 601 +                            struct tsn_psfp_sgi_status *sgi_status);
 602 +int felix_qci_fmi_set(struct net_device *ndev, u32 index,
 603 +                     bool enable, struct tsn_qci_psfp_fmi *fmi);
 604 +int felix_qci_fmi_get(struct net_device *ndev, u32 index,
 605 +                     struct tsn_qci_psfp_fmi *fmi,
 606 +                     struct tsn_qci_psfp_fmi_counters *counters);
 607 +int felix_seq_gen_set(struct net_device *ndev, u32 index,
 608 +                     struct tsn_seq_gen_conf *sg_conf);
 609 +int felix_seq_rec_set(struct net_device *ndev, u32 index,
 610 +                     struct tsn_seq_rec_conf *sr_conf);
 611 +int felix_cb_get(struct net_device *ndev, u32 index,
 612 +                struct tsn_cb_status  *c);
 613 +int felix_dscp_set(struct net_device *ndev, bool enable, const u8 dscp_ix,
 614 +                  struct tsn_qos_switch_dscp_conf *c);
 615 +
 616 +void felix_tsn_init(struct net_device *ndev);
 617 +#endif
 618 --- a/drivers/net/dsa/ocelot/felix_vsc9959.c
 619 +++ b/drivers/net/dsa/ocelot/felix_vsc9959.c
 620 @@ -176,7 +176,7 @@ static const u32 vsc9959_qsys_regmap[] =
 621         REG(QSYS_QMAXSDU_CFG_6,                 0x00f62c),
 622         REG(QSYS_QMAXSDU_CFG_7,                 0x00f648),
 623         REG(QSYS_PREEMPTION_CFG,                0x00f664),
 624 -       REG_RESERVED(QSYS_CIR_CFG),
 625 +       REG(QSYS_CIR_CFG,                       0x000000),
 626         REG(QSYS_EIR_CFG,                       0x000004),
 627         REG(QSYS_SE_CFG,                        0x000008),
 628         REG(QSYS_SE_DWRR_CFG,                   0x00000c),
 629 @@ -269,7 +269,7 @@ static const u32 vsc9959_sys_regmap[] =
 630         REG_RESERVED(SYS_MMGT_FAST),
 631         REG_RESERVED(SYS_EVENTS_DIF),
 632         REG_RESERVED(SYS_EVENTS_CORE),
 633 -       REG_RESERVED(SYS_CNT),
 634 +       REG(SYS_CNT,                            0x000000),
 635         REG(SYS_PTP_STATUS,                     0x000f14),
 636         REG(SYS_PTP_TXSTAMP,                    0x000f18),
 637         REG(SYS_PTP_NXT,                        0x000f1c),
 638 @@ -290,6 +290,10 @@ static const u32 vsc9959_ptp_regmap[] =
 639         REG(PTP_CFG_MISC,                  0x0000a0),
 640         REG(PTP_CLK_CFG_ADJ_CFG,           0x0000a4),
 641         REG(PTP_CLK_CFG_ADJ_FREQ,          0x0000a8),
 642 +       REG(PTP_CUR_NSF,                   0x0000bc),
 643 +       REG(PTP_CUR_NSEC,                  0x0000c0),
 644 +       REG(PTP_CUR_SEC_LSB,               0x0000c4),
 645 +       REG(PTP_CUR_SEC_MSB,               0x0000c8),
 646  };
 647
 648  static const u32 vsc9959_gcb_regmap[] = {
 649 --- a/include/net/dsa.h
 650 +++ b/include/net/dsa.h
 651 @@ -545,6 +545,7 @@ struct dsa_switch_ops {
 652          */
 653         netdev_tx_t (*port_deferred_xmit)(struct dsa_switch *ds, int port,
 654                                           struct sk_buff *skb);
 655 +       int     (*port_tsn_enable)(struct dsa_port *dp);
 656  };
 657
 658  struct dsa_switch_driver {
 659 --- a/net/dsa/dsa2.c
 660 +++ b/net/dsa/dsa2.c
 661 @@ -323,6 +323,10 @@ static int dsa_port_setup(struct dsa_por
 662                 if (err)
 663                         break;
 664
 665 +               /* Enable TSN function on switch port */
 666 +               if (ds->ops->port_tsn_enable)
 667 +                       ds->ops->port_tsn_enable(dp);
 668 +
 669                 devlink_port_type_eth_set(dlp, dp->slave);
 670                 break;
 671         }