LXR linux/drivers/net/ethernet/stmicro/stmmac/dwmac5.c

   1// SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2// Copyright (c) 2017 Synopsys, Inc. and/or its affiliates.
   3// stmmac Support for 5.xx Ethernet QoS cores
   4
   5#include <linux/bitops.h>
   6#include <linux/iopoll.h>
   7#include "common.h"
   8#include "dwmac4.h"
   9#include "dwmac5.h"
  10#include "stmmac.h"
  11#include "stmmac_ptp.h"
  12
  13struct dwmac5_error_desc {
  14        bool valid;
  15        const char *desc;
  16        const char *detailed_desc;
  17};
  18
  19#define STAT_OFF(field)         offsetof(struct stmmac_safety_stats, field)
  20
  21static void dwmac5_log_error(struct net_device *ndev, u32 value, bool corr,
  22                const char *module_name, const struct dwmac5_error_desc *desc,
  23                unsigned long field_offset, struct stmmac_safety_stats *stats)
  24{
  25        unsigned long loc, mask;
  26        u8 *bptr = (u8 *)stats;
  27        unsigned long *ptr;
  28
  29        ptr = (unsigned long *)(bptr + field_offset);
  30
  31        mask = value;
  32        for_each_set_bit(loc, &mask, 32) {
  33                netdev_err(ndev, "Found %s error in %s: '%s: %s'\n", corr ?
  34                                "correctable" : "uncorrectable", module_name,
  35                                desc[loc].desc, desc[loc].detailed_desc);
  36
  37                /* Update counters */
  38                ptr[loc]++;
  39        }
  40}
  41
  42static const struct dwmac5_error_desc dwmac5_mac_errors[32]= {
  43        { true, "ATPES", "Application Transmit Interface Parity Check Error" },
  44        { true, "TPES", "TSO Data Path Parity Check Error" },
  45        { true, "RDPES", "Read Descriptor Parity Check Error" },
  46        { true, "MPES", "MTL Data Path Parity Check Error" },
  47        { true, "MTSPES", "MTL TX Status Data Path Parity Check Error" },
  48        { true, "ARPES", "Application Receive Interface Data Path Parity Check Error" },
  49        { true, "CWPES", "CSR Write Data Path Parity Check Error" },
  50        { true, "ASRPES", "AXI Slave Read Data Path Parity Check Error" },
  51        { true, "TTES", "TX FSM Timeout Error" },
  52        { true, "RTES", "RX FSM Timeout Error" },
  53        { true, "CTES", "CSR FSM Timeout Error" },
  54        { true, "ATES", "APP FSM Timeout Error" },
  55        { true, "PTES", "PTP FSM Timeout Error" },
  56        { true, "T125ES", "TX125 FSM Timeout Error" },
  57        { true, "R125ES", "RX125 FSM Timeout Error" },
  58        { true, "RVCTES", "REV MDC FSM Timeout Error" },
  59        { true, "MSTTES", "Master Read/Write Timeout Error" },
  60        { true, "SLVTES", "Slave Read/Write Timeout Error" },
  61        { true, "ATITES", "Application Timeout on ATI Interface Error" },
  62        { true, "ARITES", "Application Timeout on ARI Interface Error" },
  63        { false, "UNKNOWN", "Unknown Error" }, /* 20 */
  64        { false, "UNKNOWN", "Unknown Error" }, /* 21 */
  65        { false, "UNKNOWN", "Unknown Error" }, /* 22 */
  66        { false, "UNKNOWN", "Unknown Error" }, /* 23 */
  67        { true, "FSMPES", "FSM State Parity Error" },
  68        { false, "UNKNOWN", "Unknown Error" }, /* 25 */
  69        { false, "UNKNOWN", "Unknown Error" }, /* 26 */
  70        { false, "UNKNOWN", "Unknown Error" }, /* 27 */
  71        { false, "UNKNOWN", "Unknown Error" }, /* 28 */
  72        { false, "UNKNOWN", "Unknown Error" }, /* 29 */
  73        { false, "UNKNOWN", "Unknown Error" }, /* 30 */
  74        { false, "UNKNOWN", "Unknown Error" }, /* 31 */
  75};
  76
  77static void dwmac5_handle_mac_err(struct net_device *ndev,
  78                void __iomem *ioaddr, bool correctable,
  79                struct stmmac_safety_stats *stats)
  80{
  81        u32 value;
  82
  83        value = readl(ioaddr + MAC_DPP_FSM_INT_STATUS);
  84        writel(value, ioaddr + MAC_DPP_FSM_INT_STATUS);
  85
  86        dwmac5_log_error(ndev, value, correctable, "MAC", dwmac5_mac_errors,
  87                        STAT_OFF(mac_errors), stats);
  88}
  89
  90static const struct dwmac5_error_desc dwmac5_mtl_errors[32]= {
  91        { true, "TXCES", "MTL TX Memory Error" },
  92        { true, "TXAMS", "MTL TX Memory Address Mismatch Error" },
  93        { true, "TXUES", "MTL TX Memory Error" },
  94        { false, "UNKNOWN", "Unknown Error" }, /* 3 */
  95        { true, "RXCES", "MTL RX Memory Error" },
  96        { true, "RXAMS", "MTL RX Memory Address Mismatch Error" },
  97        { true, "RXUES", "MTL RX Memory Error" },
  98        { false, "UNKNOWN", "Unknown Error" }, /* 7 */
  99        { true, "ECES", "MTL EST Memory Error" },
 100        { true, "EAMS", "MTL EST Memory Address Mismatch Error" },
 101        { true, "EUES", "MTL EST Memory Error" },
 102        { false, "UNKNOWN", "Unknown Error" }, /* 11 */
 103        { true, "RPCES", "MTL RX Parser Memory Error" },
 104        { true, "RPAMS", "MTL RX Parser Memory Address Mismatch Error" },
 105        { true, "RPUES", "MTL RX Parser Memory Error" },
 106        { false, "UNKNOWN", "Unknown Error" }, /* 15 */
 107        { false, "UNKNOWN", "Unknown Error" }, /* 16 */
 108        { false, "UNKNOWN", "Unknown Error" }, /* 17 */
 109        { false, "UNKNOWN", "Unknown Error" }, /* 18 */
 110        { false, "UNKNOWN", "Unknown Error" }, /* 19 */
 111        { false, "UNKNOWN", "Unknown Error" }, /* 20 */
 112        { false, "UNKNOWN", "Unknown Error" }, /* 21 */
 113        { false, "UNKNOWN", "Unknown Error" }, /* 22 */
 114        { false, "UNKNOWN", "Unknown Error" }, /* 23 */
 115        { false, "UNKNOWN", "Unknown Error" }, /* 24 */
 116        { false, "UNKNOWN", "Unknown Error" }, /* 25 */
 117        { false, "UNKNOWN", "Unknown Error" }, /* 26 */
 118        { false, "UNKNOWN", "Unknown Error" }, /* 27 */
 119        { false, "UNKNOWN", "Unknown Error" }, /* 28 */
 120        { false, "UNKNOWN", "Unknown Error" }, /* 29 */
 121        { false, "UNKNOWN", "Unknown Error" }, /* 30 */
 122        { false, "UNKNOWN", "Unknown Error" }, /* 31 */
 123};
 124
 125static void dwmac5_handle_mtl_err(struct net_device *ndev,
 126                void __iomem *ioaddr, bool correctable,
 127                struct stmmac_safety_stats *stats)
 128{
 129        u32 value;
 130
 131        value = readl(ioaddr + MTL_ECC_INT_STATUS);
 132        writel(value, ioaddr + MTL_ECC_INT_STATUS);
 133
 134        dwmac5_log_error(ndev, value, correctable, "MTL", dwmac5_mtl_errors,
 135                        STAT_OFF(mtl_errors), stats);
 136}
 137
 138static const struct dwmac5_error_desc dwmac5_dma_errors[32]= {
 139        { true, "TCES", "DMA TSO Memory Error" },
 140        { true, "TAMS", "DMA TSO Memory Address Mismatch Error" },
 141        { true, "TUES", "DMA TSO Memory Error" },
 142        { false, "UNKNOWN", "Unknown Error" }, /* 3 */
 143        { false, "UNKNOWN", "Unknown Error" }, /* 4 */
 144        { false, "UNKNOWN", "Unknown Error" }, /* 5 */
 145        { false, "UNKNOWN", "Unknown Error" }, /* 6 */
 146        { false, "UNKNOWN", "Unknown Error" }, /* 7 */
 147        { false, "UNKNOWN", "Unknown Error" }, /* 8 */
 148        { false, "UNKNOWN", "Unknown Error" }, /* 9 */
 149        { false, "UNKNOWN", "Unknown Error" }, /* 10 */
 150        { false, "UNKNOWN", "Unknown Error" }, /* 11 */
 151        { false, "UNKNOWN", "Unknown Error" }, /* 12 */
 152        { false, "UNKNOWN", "Unknown Error" }, /* 13 */
 153        { false, "UNKNOWN", "Unknown Error" }, /* 14 */
 154        { false, "UNKNOWN", "Unknown Error" }, /* 15 */
 155        { false, "UNKNOWN", "Unknown Error" }, /* 16 */
 156        { false, "UNKNOWN", "Unknown Error" }, /* 17 */
 157        { false, "UNKNOWN", "Unknown Error" }, /* 18 */
 158        { false, "UNKNOWN", "Unknown Error" }, /* 19 */
 159        { false, "UNKNOWN", "Unknown Error" }, /* 20 */
 160        { false, "UNKNOWN", "Unknown Error" }, /* 21 */
 161        { false, "UNKNOWN", "Unknown Error" }, /* 22 */
 162        { false, "UNKNOWN", "Unknown Error" }, /* 23 */
 163        { false, "UNKNOWN", "Unknown Error" }, /* 24 */
 164        { false, "UNKNOWN", "Unknown Error" }, /* 25 */
 165        { false, "UNKNOWN", "Unknown Error" }, /* 26 */
 166        { false, "UNKNOWN", "Unknown Error" }, /* 27 */
 167        { false, "UNKNOWN", "Unknown Error" }, /* 28 */
 168        { false, "UNKNOWN", "Unknown Error" }, /* 29 */
 169        { false, "UNKNOWN", "Unknown Error" }, /* 30 */
 170        { false, "UNKNOWN", "Unknown Error" }, /* 31 */
 171};
 172
 173static void dwmac5_handle_dma_err(struct net_device *ndev,
 174                void __iomem *ioaddr, bool correctable,
 175                struct stmmac_safety_stats *stats)
 176{
 177        u32 value;
 178
 179        value = readl(ioaddr + DMA_ECC_INT_STATUS);
 180        writel(value, ioaddr + DMA_ECC_INT_STATUS);
 181
 182        dwmac5_log_error(ndev, value, correctable, "DMA", dwmac5_dma_errors,
 183                        STAT_OFF(dma_errors), stats);
 184}
 185
 186int dwmac5_safety_feat_config(void __iomem *ioaddr, unsigned int asp)
 187{
 188        u32 value;
 189
 190        if (!asp)
 191                return -EINVAL;
 192
 193        /* 1. Enable Safety Features */
 194        value = readl(ioaddr + MTL_ECC_CONTROL);
 195        value |= TSOEE; /* TSO ECC */
 196        value |= MRXPEE; /* MTL RX Parser ECC */
 197        value |= MESTEE; /* MTL EST ECC */
 198        value |= MRXEE; /* MTL RX FIFO ECC */
 199        value |= MTXEE; /* MTL TX FIFO ECC */
 200        writel(value, ioaddr + MTL_ECC_CONTROL);
 201
 202        /* 2. Enable MTL Safety Interrupts */
 203        value = readl(ioaddr + MTL_ECC_INT_ENABLE);
 204        value |= RPCEIE; /* RX Parser Memory Correctable Error */
 205        value |= ECEIE; /* EST Memory Correctable Error */
 206        value |= RXCEIE; /* RX Memory Correctable Error */
 207        value |= TXCEIE; /* TX Memory Correctable Error */
 208        writel(value, ioaddr + MTL_ECC_INT_ENABLE);
 209
 210        /* 3. Enable DMA Safety Interrupts */
 211        value = readl(ioaddr + DMA_ECC_INT_ENABLE);
 212        value |= TCEIE; /* TSO Memory Correctable Error */
 213        writel(value, ioaddr + DMA_ECC_INT_ENABLE);
 214
 215        /* Only ECC Protection for External Memory feature is selected */
 216        if (asp <= 0x1)
 217                return 0;
 218
 219        /* 5. Enable Parity and Timeout for FSM */
 220        value = readl(ioaddr + MAC_FSM_CONTROL);
 221        value |= PRTYEN; /* FSM Parity Feature */
 222        value |= TMOUTEN; /* FSM Timeout Feature */
 223        writel(value, ioaddr + MAC_FSM_CONTROL);
 224
 225        /* 4. Enable Data Parity Protection */
 226        value = readl(ioaddr + MTL_DPP_CONTROL);
 227        value |= EDPP;
 228        writel(value, ioaddr + MTL_DPP_CONTROL);
 229
 230        /*
 231         * All the Automotive Safety features are selected without the "Parity
 232         * Port Enable for external interface" feature.
 233         */
 234        if (asp <= 0x2)
 235                return 0;
 236
 237        value |= EPSI;
 238        writel(value, ioaddr + MTL_DPP_CONTROL);
 239        return 0;
 240}
 241
 242int dwmac5_safety_feat_irq_status(struct net_device *ndev,
 243                void __iomem *ioaddr, unsigned int asp,
 244                struct stmmac_safety_stats *stats)
 245{
 246        bool err, corr;
 247        u32 mtl, dma;
 248        int ret = 0;
 249
 250        if (!asp)
 251                return -EINVAL;
 252
 253        mtl = readl(ioaddr + MTL_SAFETY_INT_STATUS);
 254        dma = readl(ioaddr + DMA_SAFETY_INT_STATUS);
 255
 256        err = (mtl & MCSIS) || (dma & MCSIS);
 257        corr = false;
 258        if (err) {
 259                dwmac5_handle_mac_err(ndev, ioaddr, corr, stats);
 260                ret |= !corr;
 261        }
 262
 263        err = (mtl & (MEUIS | MECIS)) || (dma & (MSUIS | MSCIS));
 264        corr = (mtl & MECIS) || (dma & MSCIS);
 265        if (err) {
 266                dwmac5_handle_mtl_err(ndev, ioaddr, corr, stats);
 267                ret |= !corr;
 268        }
 269
 270        err = dma & (DEUIS | DECIS);
 271        corr = dma & DECIS;
 272        if (err) {
 273                dwmac5_handle_dma_err(ndev, ioaddr, corr, stats);
 274                ret |= !corr;
 275        }
 276
 277        return ret;
 278}
 279
 280static const struct dwmac5_error {
 281        const struct dwmac5_error_desc *desc;
 282} dwmac5_all_errors[] = {
 283        { dwmac5_mac_errors },
 284        { dwmac5_mtl_errors },
 285        { dwmac5_dma_errors },
 286};
 287
 288int dwmac5_safety_feat_dump(struct stmmac_safety_stats *stats,
 289                        int index, unsigned long *count, const char **desc)
 290{
 291        int module = index / 32, offset = index % 32;
 292        unsigned long *ptr = (unsigned long *)stats;
 293
 294        if (module >= ARRAY_SIZE(dwmac5_all_errors))
 295                return -EINVAL;
 296        if (!dwmac5_all_errors[module].desc[offset].valid)
 297                return -EINVAL;
 298        if (count)
 299                *count = *(ptr + index);
 300        if (desc)
 301                *desc = dwmac5_all_errors[module].desc[offset].desc;
 302        return 0;
 303}
 304
 305static int dwmac5_rxp_disable(void __iomem *ioaddr)
 306{
 307        u32 val;
 308        int ret;
 309
 310        val = readl(ioaddr + MTL_OPERATION_MODE);
 311        val &= ~MTL_FRPE;
 312        writel(val, ioaddr + MTL_OPERATION_MODE);
 313
 314        ret = readl_poll_timeout(ioaddr + MTL_RXP_CONTROL_STATUS, val,
 315                        val & RXPI, 1, 10000);
 316        if (ret)
 317                return ret;
 318        return 0;
 319}
 320
 321static void dwmac5_rxp_enable(void __iomem *ioaddr)
 322{
 323        u32 val;
 324
 325        val = readl(ioaddr + MTL_OPERATION_MODE);
 326        val |= MTL_FRPE;
 327        writel(val, ioaddr + MTL_OPERATION_MODE);
 328}
 329
 330static int dwmac5_rxp_update_single_entry(void __iomem *ioaddr,
 331                                          struct stmmac_tc_entry *entry,
 332                                          int pos)
 333{
 334        int ret, i;
 335
 336        for (i = 0; i < (sizeof(entry->val) / sizeof(u32)); i++) {
 337                int real_pos = pos * (sizeof(entry->val) / sizeof(u32)) + i;
 338                u32 val;
 339
 340                /* Wait for ready */
 341                ret = readl_poll_timeout(ioaddr + MTL_RXP_IACC_CTRL_STATUS,
 342                                val, !(val & STARTBUSY), 1, 10000);
 343                if (ret)
 344                        return ret;
 345
 346                /* Write data */
 347                val = *((u32 *)&entry->val + i);
 348                writel(val, ioaddr + MTL_RXP_IACC_DATA);
 349
 350                /* Write pos */
 351                val = real_pos & ADDR;
 352                writel(val, ioaddr + MTL_RXP_IACC_CTRL_STATUS);
 353
 354                /* Write OP */
 355                val |= WRRDN;
 356                writel(val, ioaddr + MTL_RXP_IACC_CTRL_STATUS);
 357
 358                /* Start Write */
 359                val |= STARTBUSY;
 360                writel(val, ioaddr + MTL_RXP_IACC_CTRL_STATUS);
 361
 362                /* Wait for done */
 363                ret = readl_poll_timeout(ioaddr + MTL_RXP_IACC_CTRL_STATUS,
 364                                val, !(val & STARTBUSY), 1, 10000);
 365                if (ret)
 366                        return ret;
 367        }
 368
 369        return 0;
 370}
 371
 372static struct stmmac_tc_entry *
 373dwmac5_rxp_get_next_entry(struct stmmac_tc_entry *entries, unsigned int count,
 374                          u32 curr_prio)
 375{
 376        struct stmmac_tc_entry *entry;
 377        u32 min_prio = ~0x0;
 378        int i, min_prio_idx;
 379        bool found = false;
 380
 381        for (i = count - 1; i >= 0; i--) {
 382                entry = &entries[i];
 383
 384                /* Do not update unused entries */
 385                if (!entry->in_use)
 386                        continue;
 387                /* Do not update already updated entries (i.e. fragments) */
 388                if (entry->in_hw)
 389                        continue;
 390                /* Let last entry be updated last */
 391                if (entry->is_last)
 392                        continue;
 393                /* Do not return fragments */
 394                if (entry->is_frag)
 395                        continue;
 396                /* Check if we already checked this prio */
 397                if (entry->prio < curr_prio)
 398                        continue;
 399                /* Check if this is the minimum prio */
 400                if (entry->prio < min_prio) {
 401                        min_prio = entry->prio;
 402                        min_prio_idx = i;
 403                        found = true;
 404                }
 405        }
 406
 407        if (found)
 408                return &entries[min_prio_idx];
 409        return NULL;
 410}
 411
 412int dwmac5_rxp_config(void __iomem *ioaddr, struct stmmac_tc_entry *entries,
 413                      unsigned int count)
 414{
 415        struct stmmac_tc_entry *entry, *frag;
 416        int i, ret, nve = 0;
 417        u32 curr_prio = 0;
 418        u32 old_val, val;
 419
 420        /* Force disable RX */
 421        old_val = readl(ioaddr + GMAC_CONFIG);
 422        val = old_val & ~GMAC_CONFIG_RE;
 423        writel(val, ioaddr + GMAC_CONFIG);
 424
 425        /* Disable RX Parser */
 426        ret = dwmac5_rxp_disable(ioaddr);
 427        if (ret)
 428                goto re_enable;
 429
 430        /* Set all entries as NOT in HW */
 431        for (i = 0; i < count; i++) {
 432                entry = &entries[i];
 433                entry->in_hw = false;
 434        }
 435
 436        /* Update entries by reverse order */
 437        while (1) {
 438                entry = dwmac5_rxp_get_next_entry(entries, count, curr_prio);
 439                if (!entry)
 440                        break;
 441
 442                curr_prio = entry->prio;
 443                frag = entry->frag_ptr;
 444
 445                /* Set special fragment requirements */
 446                if (frag) {
 447                        entry->val.af = 0;
 448                        entry->val.rf = 0;
 449                        entry->val.nc = 1;
 450                        entry->val.ok_index = nve + 2;
 451                }
 452
 453                ret = dwmac5_rxp_update_single_entry(ioaddr, entry, nve);
 454                if (ret)
 455                        goto re_enable;
 456
 457                entry->table_pos = nve++;
 458                entry->in_hw = true;
 459
 460                if (frag && !frag->in_hw) {
 461                        ret = dwmac5_rxp_update_single_entry(ioaddr, frag, nve);
 462                        if (ret)
 463                                goto re_enable;
 464                        frag->table_pos = nve++;
 465                        frag->in_hw = true;
 466                }
 467        }
 468
 469        if (!nve)
 470                goto re_enable;
 471
 472        /* Update all pass entry */
 473        for (i = 0; i < count; i++) {
 474                entry = &entries[i];
 475                if (!entry->is_last)
 476                        continue;
 477
 478                ret = dwmac5_rxp_update_single_entry(ioaddr, entry, nve);
 479                if (ret)
 480                        goto re_enable;
 481
 482                entry->table_pos = nve++;
 483        }
 484
 485        /* Assume n. of parsable entries == n. of valid entries */
 486        val = (nve << 16) & NPE;
 487        val |= nve & NVE;
 488        writel(val, ioaddr + MTL_RXP_CONTROL_STATUS);
 489
 490        /* Enable RX Parser */
 491        dwmac5_rxp_enable(ioaddr);
 492
 493re_enable:
 494        /* Re-enable RX */
 495        writel(old_val, ioaddr + GMAC_CONFIG);
 496        return ret;
 497}
 498
 499int dwmac5_flex_pps_config(void __iomem *ioaddr, int index,
 500                           struct stmmac_pps_cfg *cfg, bool enable,
 501                           u32 sub_second_inc, u32 systime_flags)
 502{
 503        u32 tnsec = readl(ioaddr + MAC_PPSx_TARGET_TIME_NSEC(index));
 504        u32 val = readl(ioaddr + MAC_PPS_CONTROL);
 505        u64 period;
 506
 507        if (!cfg->available)
 508                return -EINVAL;
 509        if (tnsec & TRGTBUSY0)
 510                return -EBUSY;
 511        if (!sub_second_inc || !systime_flags)
 512                return -EINVAL;
 513
 514        val &= ~PPSx_MASK(index);
 515
 516        if (!enable) {
 517                val |= PPSCMDx(index, 0x5);
 518                val |= PPSEN0;
 519                writel(val, ioaddr + MAC_PPS_CONTROL);
 520                return 0;
 521        }
 522
 523        val |= PPSCMDx(index, 0x2);
 524        val |= TRGTMODSELx(index, 0x2);
 525        val |= PPSEN0;
 526
 527        writel(cfg->start.tv_sec, ioaddr + MAC_PPSx_TARGET_TIME_SEC(index));
 528
 529        if (!(systime_flags & PTP_TCR_TSCTRLSSR))
 530                cfg->start.tv_nsec = (cfg->start.tv_nsec * 1000) / 465;
 531        writel(cfg->start.tv_nsec, ioaddr + MAC_PPSx_TARGET_TIME_NSEC(index));
 532
 533        period = cfg->period.tv_sec * 1000000000;
 534        period += cfg->period.tv_nsec;
 535
 536        do_div(period, sub_second_inc);
 537
 538        if (period <= 1)
 539                return -EINVAL;
 540
 541        writel(period - 1, ioaddr + MAC_PPSx_INTERVAL(index));
 542
 543        period >>= 1;
 544        if (period <= 1)
 545                return -EINVAL;
 546
 547        writel(period - 1, ioaddr + MAC_PPSx_WIDTH(index));
 548
 549        /* Finally, activate it */
 550        writel(val, ioaddr + MAC_PPS_CONTROL);
 551        return 0;
 552}
 553
 554static int dwmac5_est_write(void __iomem *ioaddr, u32 reg, u32 val, bool gcl)
 555{
 556        u32 ctrl;
 557
 558        writel(val, ioaddr + MTL_EST_GCL_DATA);
 559
 560        ctrl = (reg << ADDR_SHIFT);
 561        ctrl |= gcl ? 0 : GCRR;
 562
 563        writel(ctrl, ioaddr + MTL_EST_GCL_CONTROL);
 564
 565        ctrl |= SRWO;
 566        writel(ctrl, ioaddr + MTL_EST_GCL_CONTROL);
 567
 568        return readl_poll_timeout(ioaddr + MTL_EST_GCL_CONTROL,
 569                                  ctrl, !(ctrl & SRWO), 100, 5000);
 570}
 571
 572int dwmac5_est_configure(void __iomem *ioaddr, struct stmmac_est *cfg,
 573                         unsigned int ptp_rate)
 574{
 575        u32 speed, total_offset, offset, ctrl, ctr_low;
 576        u32 extcfg = readl(ioaddr + GMAC_EXT_CONFIG);
 577        u32 mac_cfg = readl(ioaddr + GMAC_CONFIG);
 578        int i, ret = 0x0;
 579        u64 total_ctr;
 580
 581        if (extcfg & GMAC_CONFIG_EIPG_EN) {
 582                offset = (extcfg & GMAC_CONFIG_EIPG) >> GMAC_CONFIG_EIPG_SHIFT;
 583                offset = 104 + (offset * 8);
 584        } else {
 585                offset = (mac_cfg & GMAC_CONFIG_IPG) >> GMAC_CONFIG_IPG_SHIFT;
 586                offset = 96 - (offset * 8);
 587        }
 588
 589        speed = mac_cfg & (GMAC_CONFIG_PS | GMAC_CONFIG_FES);
 590        speed = speed >> GMAC_CONFIG_FES_SHIFT;
 591
 592        switch (speed) {
 593        case 0x0:
 594                offset = offset * 1000; /* 1G */
 595                break;
 596        case 0x1:
 597                offset = offset * 400; /* 2.5G */
 598                break;
 599        case 0x2:
 600                offset = offset * 100000; /* 10M */
 601                break;
 602        case 0x3:
 603                offset = offset * 10000; /* 100M */
 604                break;
 605        default:
 606                return -EINVAL;
 607        }
 608
 609        offset = offset / 1000;
 610
 611        ret |= dwmac5_est_write(ioaddr, BTR_LOW, cfg->btr[0], false);
 612        ret |= dwmac5_est_write(ioaddr, BTR_HIGH, cfg->btr[1], false);
 613        ret |= dwmac5_est_write(ioaddr, TER, cfg->ter, false);
 614        ret |= dwmac5_est_write(ioaddr, LLR, cfg->gcl_size, false);
 615        if (ret)
 616                return ret;
 617
 618        total_offset = 0;
 619        for (i = 0; i < cfg->gcl_size; i++) {
 620                ret = dwmac5_est_write(ioaddr, i, cfg->gcl[i] + offset, true);
 621                if (ret)
 622                        return ret;
 623
 624                total_offset += offset;
 625        }
 626
 627        total_ctr = cfg->ctr[0] + cfg->ctr[1] * 1000000000ULL;
 628        total_ctr += total_offset;
 629
 630        ctr_low = do_div(total_ctr, 1000000000);
 631
 632        ret |= dwmac5_est_write(ioaddr, CTR_LOW, ctr_low, false);
 633        ret |= dwmac5_est_write(ioaddr, CTR_HIGH, total_ctr, false);
 634        if (ret)
 635                return ret;
 636
 637        ctrl = readl(ioaddr + MTL_EST_CONTROL);
 638        ctrl &= ~PTOV;
 639        ctrl |= ((1000000000 / ptp_rate) * 6) << PTOV_SHIFT;
 640        if (cfg->enable)
 641                ctrl |= EEST | SSWL;
 642        else
 643                ctrl &= ~EEST;
 644
 645        writel(ctrl, ioaddr + MTL_EST_CONTROL);
 646        return 0;
 647}
 648
 649void dwmac5_fpe_configure(void __iomem *ioaddr, u32 num_txq, u32 num_rxq,
 650                          bool enable)
 651{
 652        u32 value;
 653
 654        if (!enable) {
 655                value = readl(ioaddr + MAC_FPE_CTRL_STS);
 656
 657                value &= ~EFPE;
 658
 659                writel(value, ioaddr + MAC_FPE_CTRL_STS);
 660                return;
 661        }
 662
 663        value = readl(ioaddr + GMAC_RXQ_CTRL1);
 664        value &= ~GMAC_RXQCTRL_FPRQ;
 665        value |= (num_rxq - 1) << GMAC_RXQCTRL_FPRQ_SHIFT;
 666        writel(value, ioaddr + GMAC_RXQ_CTRL1);
 667
 668        value = readl(ioaddr + MAC_FPE_CTRL_STS);
 669        value |= EFPE;
 670        writel(value, ioaddr + MAC_FPE_CTRL_STS);
 671}
 672