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         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
 321 static 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
 330 static 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
 372 static struct stmmac_tc_entry *
 373 dwmac5_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
 412 int 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
 493 re_enable:
 494         /* Re-enable RX */
 495         writel(old_val, ioaddr + GMAC_CONFIG);
 496         return ret;
 497 }
 498
 499 int 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
 554 static 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
 572 int 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
 649 void 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 }