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
  13 struct 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
  21 static 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
  42 static 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
  77 static 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
  90 static 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
 125 static 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
 138 static 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
 173 static 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
 186 int 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
 242 int 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
 280 static 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
 288 int 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
 305 static int dwmac5_rxp_disable(void __iomem *ioaddr)
 306 {
 307         u32 val;
 308
 309         val = readl(ioaddr + MTL_OPERATION_MODE);
 310         val &= ~MTL_FRPE;
 311         writel(val, ioaddr + MTL_OPERATION_MODE);
 312
 313         return readl_poll_timeout(ioaddr + MTL_RXP_CONTROL_STATUS, val,
 314                         val & RXPI, 1, 10000);
 315 }
 316
 317 static void dwmac5_rxp_enable(void __iomem *ioaddr)
 318 {
 319         u32 val;
 320
 321         val = readl(ioaddr + MTL_OPERATION_MODE);
 322         val |= MTL_FRPE;
 323         writel(val, ioaddr + MTL_OPERATION_MODE);
 324 }
 325
 326 static int dwmac5_rxp_update_single_entry(void __iomem *ioaddr,
 327                                           struct stmmac_tc_entry *entry,
 328                                           int pos)
 329 {
 330         int ret, i;
 331
 332         for (i = 0; i < (sizeof(entry->val) / sizeof(u32)); i++) {
 333                 int real_pos = pos * (sizeof(entry->val) / sizeof(u32)) + i;
 334                 u32 val;
 335
 336                 /* Wait for ready */
 337                 ret = readl_poll_timeout(ioaddr + MTL_RXP_IACC_CTRL_STATUS,
 338                                 val, !(val & STARTBUSY), 1, 10000);
 339                 if (ret)
 340                         return ret;
 341
 342                 /* Write data */
 343                 val = *((u32 *)&entry->val + i);
 344                 writel(val, ioaddr + MTL_RXP_IACC_DATA);
 345
 346                 /* Write pos */
 347                 val = real_pos & ADDR;
 348                 writel(val, ioaddr + MTL_RXP_IACC_CTRL_STATUS);
 349
 350                 /* Write OP */
 351                 val |= WRRDN;
 352                 writel(val, ioaddr + MTL_RXP_IACC_CTRL_STATUS);
 353
 354                 /* Start Write */
 355                 val |= STARTBUSY;
 356                 writel(val, ioaddr + MTL_RXP_IACC_CTRL_STATUS);
 357
 358                 /* Wait for done */
 359                 ret = readl_poll_timeout(ioaddr + MTL_RXP_IACC_CTRL_STATUS,
 360                                 val, !(val & STARTBUSY), 1, 10000);
 361                 if (ret)
 362                         return ret;
 363         }
 364
 365         return 0;
 366 }
 367
 368 static struct stmmac_tc_entry *
 369 dwmac5_rxp_get_next_entry(struct stmmac_tc_entry *entries, unsigned int count,
 370                           u32 curr_prio)
 371 {
 372         struct stmmac_tc_entry *entry;
 373         u32 min_prio = ~0x0;
 374         int i, min_prio_idx;
 375         bool found = false;
 376
 377         for (i = count - 1; i >= 0; i--) {
 378                 entry = &entries[i];
 379
 380                 /* Do not update unused entries */
 381                 if (!entry->in_use)
 382                         continue;
 383                 /* Do not update already updated entries (i.e. fragments) */
 384                 if (entry->in_hw)
 385                         continue;
 386                 /* Let last entry be updated last */
 387                 if (entry->is_last)
 388                         continue;
 389                 /* Do not return fragments */
 390                 if (entry->is_frag)
 391                         continue;
 392                 /* Check if we already checked this prio */
 393                 if (entry->prio < curr_prio)
 394                         continue;
 395                 /* Check if this is the minimum prio */
 396                 if (entry->prio < min_prio) {
 397                         min_prio = entry->prio;
 398                         min_prio_idx = i;
 399                         found = true;
 400                 }
 401         }
 402
 403         if (found)
 404                 return &entries[min_prio_idx];
 405         return NULL;
 406 }
 407
 408 int dwmac5_rxp_config(void __iomem *ioaddr, struct stmmac_tc_entry *entries,
 409                       unsigned int count)
 410 {
 411         struct stmmac_tc_entry *entry, *frag;
 412         int i, ret, nve = 0;
 413         u32 curr_prio = 0;
 414         u32 old_val, val;
 415
 416         /* Force disable RX */
 417         old_val = readl(ioaddr + GMAC_CONFIG);
 418         val = old_val & ~GMAC_CONFIG_RE;
 419         writel(val, ioaddr + GMAC_CONFIG);
 420
 421         /* Disable RX Parser */
 422         ret = dwmac5_rxp_disable(ioaddr);
 423         if (ret)
 424                 goto re_enable;
 425
 426         /* Set all entries as NOT in HW */
 427         for (i = 0; i < count; i++) {
 428                 entry = &entries[i];
 429                 entry->in_hw = false;
 430         }
 431
 432         /* Update entries by reverse order */
 433         while (1) {
 434                 entry = dwmac5_rxp_get_next_entry(entries, count, curr_prio);
 435                 if (!entry)
 436                         break;
 437
 438                 curr_prio = entry->prio;
 439                 frag = entry->frag_ptr;
 440
 441                 /* Set special fragment requirements */
 442                 if (frag) {
 443                         entry->val.af = 0;
 444                         entry->val.rf = 0;
 445                         entry->val.nc = 1;
 446                         entry->val.ok_index = nve + 2;
 447                 }
 448
 449                 ret = dwmac5_rxp_update_single_entry(ioaddr, entry, nve);
 450                 if (ret)
 451                         goto re_enable;
 452
 453                 entry->table_pos = nve++;
 454                 entry->in_hw = true;
 455
 456                 if (frag && !frag->in_hw) {
 457                         ret = dwmac5_rxp_update_single_entry(ioaddr, frag, nve);
 458                         if (ret)
 459                                 goto re_enable;
 460                         frag->table_pos = nve++;
 461                         frag->in_hw = true;
 462                 }
 463         }
 464
 465         if (!nve)
 466                 goto re_enable;
 467
 468         /* Update all pass entry */
 469         for (i = 0; i < count; i++) {
 470                 entry = &entries[i];
 471                 if (!entry->is_last)
 472                         continue;
 473
 474                 ret = dwmac5_rxp_update_single_entry(ioaddr, entry, nve);
 475                 if (ret)
 476                         goto re_enable;
 477
 478                 entry->table_pos = nve++;
 479         }
 480
 481         /* Assume n. of parsable entries == n. of valid entries */
 482         val = (nve << 16) & NPE;
 483         val |= nve & NVE;
 484         writel(val, ioaddr + MTL_RXP_CONTROL_STATUS);
 485
 486         /* Enable RX Parser */
 487         dwmac5_rxp_enable(ioaddr);
 488
 489 re_enable:
 490         /* Re-enable RX */
 491         writel(old_val, ioaddr + GMAC_CONFIG);
 492         return ret;
 493 }
 494
 495 int dwmac5_flex_pps_config(void __iomem *ioaddr, int index,
 496                            struct stmmac_pps_cfg *cfg, bool enable,
 497                            u32 sub_second_inc, u32 systime_flags)
 498 {
 499         u32 tnsec = readl(ioaddr + MAC_PPSx_TARGET_TIME_NSEC(index));
 500         u32 val = readl(ioaddr + MAC_PPS_CONTROL);
 501         u64 period;
 502
 503         if (!cfg->available)
 504                 return -EINVAL;
 505         if (tnsec & TRGTBUSY0)
 506                 return -EBUSY;
 507         if (!sub_second_inc || !systime_flags)
 508                 return -EINVAL;
 509
 510         val &= ~PPSx_MASK(index);
 511
 512         if (!enable) {
 513                 val |= PPSCMDx(index, 0x5);
 514                 val |= PPSEN0;
 515                 writel(val, ioaddr + MAC_PPS_CONTROL);
 516                 return 0;
 517         }
 518
 519         val |= PPSCMDx(index, 0x2);
 520         val |= TRGTMODSELx(index, 0x2);
 521         val |= PPSEN0;
 522
 523         writel(cfg->start.tv_sec, ioaddr + MAC_PPSx_TARGET_TIME_SEC(index));
 524
 525         if (!(systime_flags & PTP_TCR_TSCTRLSSR))
 526                 cfg->start.tv_nsec = (cfg->start.tv_nsec * 1000) / 465;
 527         writel(cfg->start.tv_nsec, ioaddr + MAC_PPSx_TARGET_TIME_NSEC(index));
 528
 529         period = cfg->period.tv_sec * 1000000000;
 530         period += cfg->period.tv_nsec;
 531
 532         do_div(period, sub_second_inc);
 533
 534         if (period <= 1)
 535                 return -EINVAL;
 536
 537         writel(period - 1, ioaddr + MAC_PPSx_INTERVAL(index));
 538
 539         period >>= 1;
 540         if (period <= 1)
 541                 return -EINVAL;
 542
 543         writel(period - 1, ioaddr + MAC_PPSx_WIDTH(index));
 544
 545         /* Finally, activate it */
 546         writel(val, ioaddr + MAC_PPS_CONTROL);
 547         return 0;
 548 }
 549
 550 static int dwmac5_est_write(void __iomem *ioaddr, u32 reg, u32 val, bool gcl)
 551 {
 552         u32 ctrl;
 553
 554         writel(val, ioaddr + MTL_EST_GCL_DATA);
 555
 556         ctrl = (reg << ADDR_SHIFT);
 557         ctrl |= gcl ? 0 : GCRR;
 558
 559         writel(ctrl, ioaddr + MTL_EST_GCL_CONTROL);
 560
 561         ctrl |= SRWO;
 562         writel(ctrl, ioaddr + MTL_EST_GCL_CONTROL);
 563
 564         return readl_poll_timeout(ioaddr + MTL_EST_GCL_CONTROL,
 565                                   ctrl, !(ctrl & SRWO), 100, 5000);
 566 }
 567
 568 int dwmac5_est_configure(void __iomem *ioaddr, struct stmmac_est *cfg,
 569                          unsigned int ptp_rate)
 570 {
 571         int i, ret = 0x0;
 572         u32 ctrl;
 573
 574         ret |= dwmac5_est_write(ioaddr, BTR_LOW, cfg->btr[0], false);
 575         ret |= dwmac5_est_write(ioaddr, BTR_HIGH, cfg->btr[1], false);
 576         ret |= dwmac5_est_write(ioaddr, TER, cfg->ter, false);
 577         ret |= dwmac5_est_write(ioaddr, LLR, cfg->gcl_size, false);
 578         ret |= dwmac5_est_write(ioaddr, CTR_LOW, cfg->ctr[0], false);
 579         ret |= dwmac5_est_write(ioaddr, CTR_HIGH, cfg->ctr[1], false);
 580         if (ret)
 581                 return ret;
 582
 583         for (i = 0; i < cfg->gcl_size; i++) {
 584                 ret = dwmac5_est_write(ioaddr, i, cfg->gcl[i], true);
 585                 if (ret)
 586                         return ret;
 587         }
 588
 589         ctrl = readl(ioaddr + MTL_EST_CONTROL);
 590         ctrl &= ~PTOV;
 591         ctrl |= ((1000000000 / ptp_rate) * 6) << PTOV_SHIFT;
 592         if (cfg->enable)
 593                 ctrl |= EEST | SSWL;
 594         else
 595                 ctrl &= ~EEST;
 596
 597         writel(ctrl, ioaddr + MTL_EST_CONTROL);
 598         return 0;
 599 }
 600
 601 void dwmac5_fpe_configure(void __iomem *ioaddr, u32 num_txq, u32 num_rxq,
 602                           bool enable)
 603 {
 604         u32 value;
 605
 606         if (!enable) {
 607                 value = readl(ioaddr + MAC_FPE_CTRL_STS);
 608
 609                 value &= ~EFPE;
 610
 611                 writel(value, ioaddr + MAC_FPE_CTRL_STS);
 612                 return;
 613         }
 614
 615         value = readl(ioaddr + GMAC_RXQ_CTRL1);
 616         value &= ~GMAC_RXQCTRL_FPRQ;
 617         value |= (num_rxq - 1) << GMAC_RXQCTRL_FPRQ_SHIFT;
 618         writel(value, ioaddr + GMAC_RXQ_CTRL1);
 619
 620         value = readl(ioaddr + MAC_FPE_CTRL_STS);
 621         value |= EFPE;
 622         writel(value, ioaddr + MAC_FPE_CTRL_STS);
 623 }