git.sur5r.net Git - freertos/blob - FreeRTOS/Demo/CORTEX_A53_64-bit_UltraScale_MPSoC/RTOSDemo_A53_bsp/psu_cortexa53_0/libsrc/usbpsu_v1_4/src/xusbpsu_hibernation.c

   1 /******************************************************************************
   2 *
   3 * Copyright (C) 2017 Xilinx, Inc.  All rights reserved.
   4 *
   5 * Permission is hereby granted, free of charge, to any person obtaining a copy
   6 * of this software and associated documentation files (the "Software"), to deal
   7 * in the Software without restriction, including without limitation the rights
   8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   9 * copies of the Software, and to permit persons to whom the Software is
  10 * furnished to do so, subject to the following conditions:
  11 *
  12 * The above copyright notice and this permission notice shall be included in
  13 * all copies or substantial portions of the Software.
  14 *
  15 * Use of the Software is limited solely to applications:
  16 * (a) running on a Xilinx device, or
  17 * (b) that interact with a Xilinx device through a bus or interconnect.
  18 *
  19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  22 * XILINX  BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  23 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
  24 * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  25 * SOFTWARE.
  26 *
  27 * Except as contained in this notice, the name of the Xilinx shall not be used
  28 * in advertising or otherwise to promote the sale, use or other dealings in
  29 * this Software without prior written authorization from Xilinx.
  30 *
  31 ******************************************************************************/
  32 /****************************************************************************/
  33 /**
  34 *
  35 * @file xusbpsu_hibernation.c
  36 *
  37 * This patch adds hibernation support to usbpsu driver when dwc3 is operating
  38 * as a gadget
  39 *
  40 * <pre>
  41 *
  42 * MODIFICATION HISTORY:
  43 *
  44 * Ver    Who    Date     Changes
  45 * ----- -----  -------- -----------------------------------------------------
  46 * 1.0   Mayank 12/01/18 First release
  47 *
  48 * </pre>
  49 *
  50 *****************************************************************************/
  51
  52 /***************************** Include Files ********************************/
  53
  54 #include "xusbpsu.h"
  55 #include "xusbpsu_hw.h"
  56 #include "xusbpsu_endpoint.h"
  57
  58 /************************** Constant Definitions *****************************/
  59
  60 #define NUM_OF_NONSTICKY_REGS                   27
  61
  62 #define XUSBPSU_HIBER_SCRATCHBUF_SIZE           4096U
  63
  64 #define XUSBPSU_NON_STICKY_SAVE_RETRIES         500U
  65 #define XUSBPSU_PWR_STATE_RETRIES               1500U
  66 #define XUSBPSU_CTRL_RDY_RETRIES                5000U
  67 #define XUSBPSU_TIMEOUT                         1000U
  68
  69 /**************************** Type Definitions *******************************/
  70
  71
  72 /***************** Macros (Inline Functions) Definitions *********************/
  73
  74
  75 /************************** Function Prototypes ******************************/
  76
  77
  78 /************************** Variable Definitions *****************************/
  79
  80 static u8 ScratchBuf[XUSBPSU_HIBER_SCRATCHBUF_SIZE];
  81
  82 /* Registers saved during hibernation and restored at wakeup */
  83 static u32 save_reg_addr[] = {
  84         XUSBPSU_DCTL,
  85         XUSBPSU_DCFG,
  86         XUSBPSU_DEVTEN,
  87         XUSBPSU_GSBUSCFG0,
  88         XUSBPSU_GSBUSCFG1,
  89         XUSBPSU_GCTL,
  90         XUSBPSU_GTXTHRCFG,
  91         XUSBPSU_GRXTHRCFG,
  92         XUSBPSU_GTXFIFOSIZ(0),
  93         XUSBPSU_GTXFIFOSIZ(1),
  94         XUSBPSU_GTXFIFOSIZ(2),
  95         XUSBPSU_GTXFIFOSIZ(3),
  96         XUSBPSU_GTXFIFOSIZ(4),
  97         XUSBPSU_GTXFIFOSIZ(5),
  98         XUSBPSU_GTXFIFOSIZ(6),
  99         XUSBPSU_GTXFIFOSIZ(7),
 100         XUSBPSU_GTXFIFOSIZ(8),
 101         XUSBPSU_GTXFIFOSIZ(9),
 102         XUSBPSU_GTXFIFOSIZ(10),
 103         XUSBPSU_GTXFIFOSIZ(11),
 104         XUSBPSU_GTXFIFOSIZ(12),
 105         XUSBPSU_GTXFIFOSIZ(13),
 106         XUSBPSU_GTXFIFOSIZ(14),
 107         XUSBPSU_GTXFIFOSIZ(15),
 108         XUSBPSU_GRXFIFOSIZ(0),
 109         XUSBPSU_GUSB3PIPECTL(0),
 110         XUSBPSU_GUSB2PHYCFG(0),
 111 };
 112 static u32 saved_regs[NUM_OF_NONSTICKY_REGS];
 113
 114 /*****************************************************************************/
 115 /**
 116 * Save non sticky registers
 117 *
 118 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 119 *               on.
 120 *
 121 * @return       None.
 122 *
 123 * @note         None.
 124 *
 125 ******************************************************************************/
 126 static void save_regs(struct XUsbPsu *InstancePtr)
 127 {
 128         u32 i;
 129
 130         for (i = 0; i < NUM_OF_NONSTICKY_REGS; i++)
 131                 saved_regs[i] = XUsbPsu_ReadReg(InstancePtr, save_reg_addr[i]);
 132 }
 133
 134 /*****************************************************************************/
 135 /**
 136 * Restore non sticky registers
 137 *
 138 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 139 *               on.
 140 *
 141 * @return       None.
 142 *
 143 * @note         None.
 144 *
 145 ******************************************************************************/
 146 static void restore_regs(struct XUsbPsu *InstancePtr)
 147 {
 148         u32 i;
 149
 150         for (i = 0; i < NUM_OF_NONSTICKY_REGS; i++)
 151                 XUsbPsu_WriteReg(InstancePtr, save_reg_addr[i], saved_regs[i]);
 152 }
 153
 154 /*****************************************************************************/
 155 /**
 156 * Send generic command for gadget
 157 *
 158 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 159 *               on.
 160 * @param        cmd is command to be sent
 161 * @param        param is parameter for the command, to be written in DGCMDPAR
 162 *               register
 163 *
 164 * @return
 165 *               - XST_SUCCESS on success
 166 *               - XST_FAILURE on timeout
 167 *               - XST_REGISTER_ERROR on status error
 168 *
 169 * @note         None.
 170 *
 171 ******************************************************************************/
 172 s32 XUsbPsu_SendGadgetGenericCmd(struct XUsbPsu *InstancePtr, u32 cmd,
 173                         u32 param)
 174 {
 175         u32             RegVal, retry = 500;
 176         XUsbPsu_WriteReg(InstancePtr, XUSBPSU_DGCMDPAR, param);
 177         XUsbPsu_WriteReg(InstancePtr, XUSBPSU_DGCMD, cmd | XUSBPSU_DGCMD_CMDACT);
 178
 179         do {
 180                 RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_DGCMD);
 181                 if (!(RegVal & XUSBPSU_DGCMD_CMDACT)) {
 182                         if (XUSBPSU_DGCMD_STATUS(RegVal))
 183                                 return XST_REGISTER_ERROR;
 184                         return 0;
 185                 }
 186         } while (--retry);
 187
 188         return XST_FAILURE;
 189 }
 190
 191 /*****************************************************************************/
 192 /**
 193 * Sets scratchpad buffers
 194 *
 195 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 196 *               on.
 197 *
 198 * @return       XST_SUCCESS on success or else error code
 199 *
 200 * @note         None.
 201 *
 202 ******************************************************************************/
 203 s32 XUsbPsu_SetupScratchpad(struct XUsbPsu *InstancePtr)
 204 {
 205         s32 Ret;
 206         Ret = XUsbPsu_SendGadgetGenericCmd(InstancePtr,
 207                 XUSBPSU_DGCMD_SET_SCRATCHPAD_ADDR_LO, (UINTPTR)ScratchBuf & 0xffffffff);
 208         if (Ret) {
 209                 xil_printf("Failed to set scratchpad low addr: %d\n", Ret);
 210                 return Ret;
 211         }
 212
 213         Ret = XUsbPsu_SendGadgetGenericCmd(InstancePtr,
 214                 XUSBPSU_DGCMD_SET_SCRATCHPAD_ADDR_HI, ((UINTPTR)ScratchBuf >> 16) >> 16);
 215         if (Ret) {
 216                 xil_printf("Failed to set scratchpad high addr: %d\n", Ret);
 217                 return Ret;
 218         }
 219
 220         return XST_SUCCESS;
 221 }
 222
 223 /*****************************************************************************/
 224 /**
 225 * Initialize to handle hibernation event when it comes
 226 *
 227 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 228 *               on.
 229 *
 230 * @return       none
 231 *
 232 * @note         None.
 233 *
 234 ******************************************************************************/
 235 void XUsbPsu_InitHibernation(struct XUsbPsu *InstancePtr)
 236 {
 237         u32             RegVal;
 238
 239         InstancePtr->IsHibernated = 0;
 240
 241         memset(ScratchBuf, 0, sizeof(ScratchBuf));
 242         if (InstancePtr->ConfigPtr->IsCacheCoherent == 0)
 243                 Xil_DCacheFlushRange((INTPTR)ScratchBuf, XUSBPSU_HIBER_SCRATCHBUF_SIZE);
 244
 245         XUsbPsu_SetupScratchpad(InstancePtr);
 246
 247         /* enable PHY suspend */
 248         RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_GUSB2PHYCFG(0));
 249         RegVal |= XUSBPSU_GUSB2PHYCFG_SUSPHY;
 250         XUsbPsu_WriteReg(InstancePtr, XUSBPSU_GUSB2PHYCFG(0), RegVal);
 251
 252         RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_GUSB3PIPECTL(0));
 253         RegVal |= XUSBPSU_GUSB3PIPECTL_SUSPHY;
 254         XUsbPsu_WriteReg(InstancePtr, XUSBPSU_GUSB3PIPECTL(0), RegVal);
 255 }
 256
 257 /*****************************************************************************/
 258 /**
 259 * Handle hibernation event
 260 *
 261 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 262 *               on.
 263 *
 264 * @return       none
 265 *
 266 * @note         None.
 267 *
 268 ******************************************************************************/
 269 void Xusbpsu_HibernationIntr(struct XUsbPsu *InstancePtr)
 270 {
 271         u8 EpNum;
 272         u32 RegVal;
 273         s32 retries;
 274         XusbPsuLinkState LinkState;
 275
 276         /* sanity check */
 277         switch(XUsbPsu_GetLinkState(InstancePtr)) {
 278         case XUSBPSU_LINK_STATE_SS_DIS:
 279         case XUSBPSU_LINK_STATE_U3:
 280                 break;
 281         default:
 282                 /* fake hiber interrupt */
 283                 xil_printf("got fake interrupt\r\n");
 284                 return;
 285         };
 286
 287         if (InstancePtr->Ep0State == XUSBPSU_EP0_SETUP_PHASE) {
 288                 XUsbPsu_StopTransfer(InstancePtr, 0, XUSBPSU_EP_DIR_OUT, TRUE);
 289                 XUsbPsu_RecvSetup(InstancePtr);
 290         }
 291
 292         /* stop active transfers for all endpoints including control
 293          * endpoints force rm bit should be 0 when we do this */
 294         for (EpNum = 0; EpNum < XUSBPSU_ENDPOINTS_NUM; EpNum++) {
 295                 struct XUsbPsu_Ep *Ept;
 296
 297                 Ept = &InstancePtr->eps[EpNum];
 298                 if (!Ept)
 299                         continue;
 300
 301                 if (!(Ept->EpStatus & XUSBPSU_EP_ENABLED))
 302                         continue;
 303
 304                 /* save srsource index for later use */
 305                 XUsbPsu_StopTransfer(InstancePtr, Ept->UsbEpNum,
 306                                 Ept->Direction, FALSE);
 307
 308                 XUsbPsu_SaveEndpointState(InstancePtr, Ept);
 309         }
 310
 311         /*
 312          * ack events, don't process them; h/w decrements the count by the value
 313          * written
 314          */
 315         RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_GEVNTCOUNT(0));
 316         XUsbPsu_WriteReg(InstancePtr, XUSBPSU_GEVNTCOUNT(0), RegVal);
 317         InstancePtr->Evt.Count = 0;
 318         InstancePtr->Evt.Flags &= ~XUSBPSU_EVENT_PENDING;
 319
 320         if (XUsbPsu_Stop(InstancePtr)) {
 321                 xil_printf("Failed to stop USB core\r\n");
 322                 return;
 323         }
 324
 325         RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_DCTL);
 326
 327         /* Check the link state and if it is disconnected, set
 328          * KEEP_CONNECT to 0
 329          */
 330         LinkState = XUsbPsu_GetLinkState(InstancePtr);
 331         if (LinkState == XUSBPSU_LINK_STATE_SS_DIS) {
 332                 RegVal &= ~XUSBPSU_DCTL_KEEP_CONNECT;
 333                 XUsbPsu_WriteReg(InstancePtr, XUSBPSU_DCTL, RegVal);
 334
 335                 /* update LinkState to be used while wakeup */
 336                 InstancePtr->LinkState = XUSBPSU_LINK_STATE_SS_DIS;
 337         }
 338
 339         save_regs(InstancePtr);
 340
 341         /* ask core to save state */
 342         RegVal |= XUSBPSU_DCTL_CSS;
 343         XUsbPsu_WriteReg(InstancePtr, XUSBPSU_DCTL, RegVal);
 344
 345         /* wait till core saves */
 346         if (XUsbPsu_Wait_Clear_Timeout(InstancePtr, XUSBPSU_DSTS,
 347                         XUSBPSU_DSTS_SSS, XUSBPSU_NON_STICKY_SAVE_RETRIES) == XST_FAILURE) {
 348                 xil_printf("Failed to save core state\r\n");
 349                 return;
 350         }
 351
 352         /* Enable PME to wakeup from hibernation */
 353         XUsbPsu_WriteVendorReg(XIL_PME_ENABLE, XIL_PME_ENABLE_SIG_GEN);
 354
 355         /* change power state to D3 */
 356         XUsbPsu_WriteVendorReg(XIL_REQ_PWR_STATE, XIL_REQ_PWR_STATE_D3);
 357
 358         /* wait till current state is changed to D3 */
 359         retries = XUSBPSU_PWR_STATE_RETRIES;
 360         do {
 361                 RegVal = XUsbPsu_ReadVendorReg(XIL_CUR_PWR_STATE);
 362                 if ((RegVal & XIL_CUR_PWR_STATE_BITMASK) == XIL_CUR_PWR_STATE_D3)
 363                         break;
 364
 365                 XUsbSleep(XUSBPSU_TIMEOUT);
 366         } while (--retries);
 367
 368         if (retries < 0) {
 369                 xil_printf("Failed to change power state to D3\r\n");
 370                 return;
 371         }
 372         XUsbSleep(XUSBPSU_TIMEOUT);
 373
 374         RegVal = XUsbPsu_ReadLpdReg(RST_LPD_TOP);
 375         if (InstancePtr->ConfigPtr->DeviceId == XPAR_XUSBPSU_0_DEVICE_ID)
 376                 XUsbPsu_WriteLpdReg(RST_LPD_TOP, RegVal | USB0_CORE_RST);
 377
 378         InstancePtr->IsHibernated = 1;
 379         xil_printf("Hibernated!\r\n");
 380 }
 381
 382 /*****************************************************************************/
 383 /**
 384 * Restarts transfer for active endpoint
 385 *
 386 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 387 *               on.
 388 * @param        EpNum is an endpoint number.
 389 *
 390 * @return       XST_SUCCESS on success or else XST_FAILURE.
 391 *
 392 * @note         None.
 393 *
 394 ******************************************************************************/
 395 static s32 XUsbPsu_RestartEp(struct XUsbPsu *InstancePtr, u8 EpNum)
 396 {
 397         struct XUsbPsu_EpParams *Params;
 398         struct XUsbPsu_Trb      *TrbPtr;
 399         struct XUsbPsu_Ep       *Ept;
 400         u32     Cmd;
 401         s32     Ret;
 402
 403         Xil_AssertNonvoid(InstancePtr != NULL);
 404
 405         Params = XUsbPsu_GetEpParams(InstancePtr);
 406         Xil_AssertNonvoid(Params != NULL);
 407
 408         Ept = &InstancePtr->eps[EpNum];
 409
 410         /* check if we need to restart transfer */
 411         if (!Ept->ResourceIndex && Ept->PhyEpNum)
 412                 return XST_SUCCESS;
 413
 414         if (Ept->UsbEpNum) {
 415                 TrbPtr = &Ept->EpTrb[Ept->TrbDequeue];
 416         } else {
 417                 TrbPtr = &InstancePtr->Ep0_Trb;
 418         }
 419
 420         Xil_AssertNonvoid(TrbPtr != NULL);
 421
 422         TrbPtr->Ctrl |= XUSBPSU_TRB_CTRL_HWO;
 423
 424         if (InstancePtr->ConfigPtr->IsCacheCoherent == 0) {
 425                 Xil_DCacheFlushRange((INTPTR)TrbPtr, sizeof(struct XUsbPsu_Trb));
 426                 Xil_DCacheInvalidateRange((INTPTR)Ept->BufferPtr, Ept->RequestedBytes);
 427         }
 428
 429         Params->Param0 = 0U;
 430         Params->Param1 = (UINTPTR)TrbPtr;
 431
 432         Cmd = XUSBPSU_DEPCMD_STARTTRANSFER;
 433
 434         Ret = XUsbPsu_SendEpCmd(InstancePtr, Ept->UsbEpNum, Ept->Direction,
 435                         Cmd, Params);
 436         if (Ret)
 437                 return XST_FAILURE;
 438
 439         Ept->EpStatus |= XUSBPSU_EP_BUSY;
 440         Ept->ResourceIndex = (u8)XUsbPsu_EpGetTransferIndex(InstancePtr,
 441                         Ept->UsbEpNum, Ept->Direction);
 442
 443         return XST_SUCCESS;
 444 }
 445
 446 /*****************************************************************************/
 447 /**
 448 * Restarts EP0 endpoint
 449 *
 450 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 451 *               on.
 452 *
 453 * @return       XST_SUCCESS on success or else XST_FAILURE.
 454 *
 455 * @note         None.
 456 *
 457 ******************************************************************************/
 458 static s32 XUsbPsu_RestoreEp0(struct XUsbPsu *InstancePtr)
 459 {
 460         struct XUsbPsu_Ep *Ept;
 461         s32 Ret;
 462         u8 EpNum;
 463
 464         for (EpNum = 0; EpNum < 2; EpNum++) {
 465                 Ept = &InstancePtr->eps[EpNum];
 466
 467                 if (!Ept)
 468                         continue;
 469
 470                 if (!(Ept->EpStatus & XUSBPSU_EP_ENABLED))
 471                         continue;
 472
 473                 Ret = XUsbPsu_EpEnable(InstancePtr, Ept->UsbEpNum,
 474                                 Ept->Direction, Ept->MaxSize, Ept->Type, TRUE);
 475                 if (Ret) {
 476                         xil_printf("Failed to enable EP %d on wakeup: %d\r\n",
 477                                         EpNum, Ret);
 478                         return XST_FAILURE;
 479                 }
 480
 481                 if (Ept->EpStatus & XUSBPSU_EP_STALL) {
 482                         XUsbPsu_Ep0StallRestart(InstancePtr);
 483                 } else {
 484                         Ret = XUsbPsu_RestartEp(InstancePtr, Ept->PhyEpNum);
 485                         if (Ret) {
 486                                 xil_printf("Failed to restart EP %d on wakeup: %d\r\n",
 487                                                 EpNum, Ret);
 488                                 return XST_FAILURE;
 489                         }
 490                 }
 491         }
 492
 493         return XST_SUCCESS;
 494 }
 495
 496 /*****************************************************************************/
 497 /**
 498 * Restarts non EP0 endpoints
 499 *
 500 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 501 *               on.
 502 *
 503 * @return       XST_SUCCESS on success or else XST_FAILURE.
 504 *
 505 * @note         None.
 506 *
 507 ******************************************************************************/
 508 static s32 XUsbPsu_RestoreEps(struct XUsbPsu *InstancePtr)
 509 {
 510         struct XUsbPsu_Ep *Ept;
 511         s32 Ret;
 512         u8 EpNum;
 513
 514         for (EpNum = 2; EpNum < XUSBPSU_ENDPOINTS_NUM; EpNum++) {
 515                 Ept = &InstancePtr->eps[EpNum];
 516
 517                 if (!Ept)
 518                         continue;
 519
 520                 if (!(Ept->EpStatus & XUSBPSU_EP_ENABLED))
 521                         continue;
 522
 523                 Ret = XUsbPsu_EpEnable(InstancePtr, Ept->UsbEpNum,
 524                                 Ept->Direction, Ept->MaxSize, Ept->Type, TRUE);
 525                 if (Ret) {
 526                         xil_printf("Failed to enable EP %d on wakeup: %d\r\n",
 527                                         EpNum, Ret);
 528                         return XST_FAILURE;
 529                 }
 530         }
 531
 532         for (EpNum = 2; EpNum < XUSBPSU_ENDPOINTS_NUM; EpNum++) {
 533                 Ept = &InstancePtr->eps[EpNum];
 534
 535                 if (!Ept)
 536                         continue;
 537
 538                 if (!(Ept->EpStatus & XUSBPSU_EP_ENABLED))
 539                         continue;
 540
 541                 if (Ept->EpStatus & XUSBPSU_EP_STALL) {
 542                         XUsbPsu_EpSetStall(InstancePtr, Ept->UsbEpNum, Ept->Direction);
 543                 } else {
 544                         Ret = XUsbPsu_RestartEp(InstancePtr, Ept->PhyEpNum);
 545                         if (Ret) {
 546                                 xil_printf("Failed to restart EP %d on wakeup: %d\r\n",
 547                                                 EpNum, Ret);
 548                                 return XST_FAILURE;
 549                         }
 550                 }
 551         }
 552
 553         return XST_SUCCESS;
 554 }
 555
 556 /*****************************************************************************/
 557 /**
 558 * Handle wakeup event
 559 *
 560 * @param        InstancePtr is a pointer to the XUsbPsu instance to be worked
 561 *               on.
 562 *
 563 * @return       none
 564 *
 565 * @note         None.
 566 *
 567 ******************************************************************************/
 568 void XUsbPsu_WakeupIntr(struct XUsbPsu *InstancePtr)
 569 {
 570         u32 RegVal, link_state;
 571         s32 retries;
 572         char enter_hiber;
 573
 574         RegVal = XUsbPsu_ReadLpdReg(RST_LPD_TOP);
 575         if (InstancePtr->ConfigPtr->DeviceId == XPAR_XUSBPSU_0_DEVICE_ID)
 576                 XUsbPsu_WriteLpdReg(RST_LPD_TOP, RegVal & ~USB0_CORE_RST);
 577
 578         /* change power state to D0 */
 579         XUsbPsu_WriteVendorReg(XIL_REQ_PWR_STATE, XIL_REQ_PWR_STATE_D0);
 580
 581         /* wait till current state is changed to D0 */
 582         retries = XUSBPSU_PWR_STATE_RETRIES;
 583         do {
 584                 RegVal = XUsbPsu_ReadVendorReg(XIL_CUR_PWR_STATE);
 585                 if ((RegVal & XIL_CUR_PWR_STATE_BITMASK) == XIL_CUR_PWR_STATE_D0)
 586                         break;
 587
 588                 XUsbSleep(XUSBPSU_TIMEOUT);
 589         } while (--retries);
 590
 591         if (retries < 0) {
 592                 xil_printf("Failed to change power state to D0\r\n");
 593                 return;
 594         }
 595
 596         /* ask core to restore non-sticky registers */
 597         XUsbPsu_SetupScratchpad(InstancePtr);
 598
 599         RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_DCTL);
 600         RegVal |= XUSBPSU_DCTL_CRS;
 601         XUsbPsu_WriteReg(InstancePtr, XUSBPSU_DCTL, RegVal);
 602
 603         /* wait till non-sticky registers are restored */
 604         if (XUsbPsu_Wait_Clear_Timeout(InstancePtr, XUSBPSU_DSTS,
 605                         XUSBPSU_DSTS_RSS, XUSBPSU_NON_STICKY_SAVE_RETRIES) == XST_FAILURE) {
 606                 xil_printf("Failed to restore USB core\r\n");
 607                 return;
 608         }
 609
 610         restore_regs(InstancePtr);
 611
 612         /* setup event buffers */
 613         XUsbPsu_EventBuffersSetup(InstancePtr);
 614
 615         /* nothing to do when in OTG host mode */
 616         if (XUsbPsu_ReadReg(InstancePtr, XUSBPSU_GSTS) & XUSBPSU_GSTS_CUR_MODE)
 617                 return;
 618
 619         if (XUsbPsu_RestoreEp0(InstancePtr)) {
 620                 xil_printf("Failed to restore EP0\r\n");
 621                 return;
 622         }
 623
 624         /* start controller */
 625         if (XUsbPsu_Start(InstancePtr)) {
 626                 xil_printf("Failed to start core on wakeup\r\n");
 627                 return;
 628         }
 629
 630         /* Wait until device controller is ready */
 631         if (XUsbPsu_Wait_Clear_Timeout(InstancePtr, XUSBPSU_DSTS,
 632                         XUSBPSU_DSTS_DCNRD, XUSBPSU_CTRL_RDY_RETRIES) == XST_FAILURE) {
 633                 xil_printf("Failed to ready device controller\r\n");
 634                 return;
 635         }
 636
 637         /*
 638          * there can be suprious wakeup events , so wait for some time and check
 639          * the link state
 640          */
 641         XUsbSleep(XUSBPSU_TIMEOUT * 10);
 642
 643         link_state = XUsbPsu_GetLinkState(InstancePtr);
 644
 645         switch(link_state) {
 646         case XUSBPSU_LINK_STATE_RESET:
 647                 RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_DSTS);
 648                 RegVal &= ~XUSBPSU_DCFG_DEVADDR_MASK;
 649                 XUsbPsu_WriteReg(InstancePtr, XUSBPSU_DSTS, RegVal);
 650
 651                 if (XUsbPsu_SetLinkState(InstancePtr, XUSBPSU_LINK_STATE_RECOV)) {
 652                         xil_printf("Failed to put link in Recovery\r\n");
 653                         return;
 654                 }
 655                 break;
 656         case XUSBPSU_LINK_STATE_SS_DIS:
 657                 RegVal = XUsbPsu_ReadReg(InstancePtr, XUSBPSU_DCTL);
 658                 RegVal &= ~XUSBPSU_DCTL_KEEP_CONNECT;
 659                 XUsbPsu_WriteReg(InstancePtr, XUSBPSU_DCTL, RegVal);
 660
 661                 /* fall Through */
 662         case XUSBPSU_LINK_STATE_U3:
 663                 /* enter hibernation again */
 664                 enter_hiber = 1;
 665                 break;
 666         default:
 667                 if (XUsbPsu_SetLinkState(InstancePtr, XUSBPSU_LINK_STATE_RECOV)) {
 668                         xil_printf("Failed to put link in Recovery\r\n");
 669                         return;
 670                 }
 671                 break;
 672         };
 673
 674         if (XUsbPsu_RestoreEps(InstancePtr)) {
 675                 xil_printf("Failed to restore EPs\r\n");
 676                 return;
 677         }
 678
 679         InstancePtr->IsHibernated = 0;
 680
 681         if (enter_hiber)  {
 682                 Xusbpsu_HibernationIntr(InstancePtr);
 683                 return;
 684         }
 685
 686         xil_printf("We are back from hibernation!\r\n");
 687 }
 688 /** @} */