diff --git a/EdkModulePkg/Universal/PCD/Dxe/Service.c b/EdkModulePkg/Universal/PCD/Dxe/Service.c
index ebef59590f..53d9621e8c 100644
--- a/EdkModulePkg/Universal/PCD/Dxe/Service.c
+++ b/EdkModulePkg/Universal/PCD/Dxe/Service.c
@@ -444,10 +444,10 @@ InvokeCallbackOnSet (
 
 EFI_STATUS
 SetWorker (
-  UINTN         TokenNumber,
-  VOID          *Data,
-  UINTN         Size,
-  BOOLEAN       PtrType
+  PCD_TOKEN_NUMBER        TokenNumber,
+  VOID                    *Data,
+  UINTN                   Size,
+  BOOLEAN                 PtrType
   )
 {
   UINT32              *LocalTokenNumberTable;
@@ -467,11 +467,11 @@ SetWorker (
   LocalTokenNumberTable  = IsPeiDb ? mPcdDatabase->PeiDb.Init.LocalTokenNumberTable : 
                                      mPcdDatabase->DxeDb.Init.LocalTokenNumberTable;
 
+  InvokeCallbackOnSet (0, NULL, TokenNumber, Data, Size);
+
   TokenNumber = IsPeiDb ? TokenNumber
                         : TokenNumber - PEI_LOCAL_TOKEN_NUMBER;
   
-  InvokeCallbackOnSet (0, NULL, TokenNumber, Data, Size);
-
   return SetWorkerByLocalTokenNumber (LocalTokenNumberTable[TokenNumber], Data, Size, PtrType, IsPeiDb);
 
 }
@@ -505,7 +505,7 @@ ExGetWorker (
 
 EFI_STATUS
 ExSetWorker (
-  IN UINT32               ExTokenNumber,
+  IN PCD_TOKEN_NUMBER     ExTokenNumber,
   IN CONST EFI_GUID       *Guid,
   VOID                    *Data,
   UINTN                   SetSize,
@@ -683,7 +683,7 @@ SetHiiVariable (
 VOID
 GetExPcdTokenAttributes (
   IN CONST EFI_GUID             *Guid,
-  IN UINT32                     ExTokenNumber,
+  IN PCD_TOKEN_NUMBER           ExTokenNumber,
   OUT EX_PCD_ENTRY_ATTRIBUTE    *ExAttr
   )
 {
@@ -721,7 +721,7 @@ GetExPcdTokenAttributes (
 
         ExAttr->IsPeiDb               = FALSE;
         ExAttr->Size                  = SizeTable[i + DXE_NEX_TOKEN_NUMBER];
-        ExAttr->TokenNumber           = i + DXE_NEX_TOKEN_NUMBER;
+        ExAttr->TokenNumber           = i + PEI_LOCAL_TOKEN_NUMBER;
         ExAttr->LocalTokenNumberAlias = ExMap[i].LocalTokenNumber;
         return;
 
diff --git a/EdkModulePkg/Universal/PCD/Dxe/Service.h b/EdkModulePkg/Universal/PCD/Dxe/Service.h
index d33a891eba..ec51ebbec8 100644
--- a/EdkModulePkg/Universal/PCD/Dxe/Service.h
+++ b/EdkModulePkg/Universal/PCD/Dxe/Service.h
@@ -46,16 +46,16 @@ typedef struct {
 
 EFI_STATUS
 SetWorker (
-  UINTN         TokenNumber,
-  VOID          *Data,
-  UINTN         Size,
-  BOOLEAN       PtrType
+  IN PCD_TOKEN_NUMBER          TokenNumber,
+  IN VOID                      *Data,
+  IN UINTN                     Size,
+  IN BOOLEAN                   PtrType
   )
 ;
 
 EFI_STATUS
 ExSetWorker (
-  IN UINT32               ExTokenNumber,
+  IN PCD_TOKEN_NUMBER     ExTokenNumber,
   IN CONST EFI_GUID       *Guid,
   VOID                    *Data,
   UINTN                   Size,
@@ -66,14 +66,14 @@ ExSetWorker (
 
 VOID *
 GetWorker (
-  UINTN  TokenNumber
+  PCD_TOKEN_NUMBER  TokenNumber
   )
 ;
 
 VOID *
 ExGetWorker (
   IN CONST EFI_GUID         *Guid,
-  IN UINTN                  ExTokenNumber,
+  IN PCD_TOKEN_NUMBER       ExTokenNumber,
   IN UINTN                  GetSize
   ) 
 ;
@@ -90,21 +90,21 @@ EFI_STATUS
 GetHiiVariable (
   IN  EFI_GUID      *VariableGuid,
   IN  UINT16        *VariableName,
-  OUT VOID          ** VariableData,
+  OUT VOID          **VariableData,
   OUT UINTN         *VariableSize
   )
 ;
 
 EFI_STATUS
 DxeRegisterCallBackWorker (
-  IN  UINTN                   TokenNumber,
+  IN  PCD_TOKEN_NUMBER        TokenNumber,
   IN  CONST EFI_GUID          *Guid, OPTIONAL
   IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
 );
 
 EFI_STATUS
 DxeUnRegisterCallBackWorker (
-  IN  UINTN                   TokenNumber,
+  IN  PCD_TOKEN_NUMBER        TokenNumber,
   IN  CONST EFI_GUID          *Guid, OPTIONAL
   IN  PCD_PROTOCOL_CALLBACK   CallBackFunction
 );
@@ -125,7 +125,7 @@ typedef struct {
 VOID
 GetExPcdTokenAttributes (
   IN CONST EFI_GUID             *Guid,
-  IN UINT32                     ExTokenNumber,
+  IN PCD_TOKEN_NUMBER           ExTokenNumber,
   OUT EX_PCD_ENTRY_ATTRIBUTE    *ExAttr
   )
 ;
diff --git a/EdkModulePkg/Universal/PCD/Pei/Pcd.c b/EdkModulePkg/Universal/PCD/Pei/Pcd.c
index 031259b22a..f084475335 100644
--- a/EdkModulePkg/Universal/PCD/Pei/Pcd.c
+++ b/EdkModulePkg/Universal/PCD/Pei/Pcd.c
@@ -500,13 +500,13 @@ PeiPcdGetNextToken (
     (*TokenNumber)++;
 
     if (*TokenNumber >= PEI_NEX_TOKEN_NUMBER) {
-      *TokenNumber = 0;
+      *TokenNumber = PCD_INVALID_TOKEN_NUMBER;
     }
     
   } else {
 
     if (PEI_EXMAP_TABLE_EMPTY) {
-      *TokenNumber = (UINTN) PCD_INVALID_TOKEN_NUMBER;
+      *TokenNumber = PCD_INVALID_TOKEN_NUMBER;
       return EFI_NOT_FOUND;
     }
     
diff --git a/EdkModulePkg/Universal/PCD/Pei/Service.c b/EdkModulePkg/Universal/PCD/Pei/Service.c
index 30595e1323..e280dd6715 100644
--- a/EdkModulePkg/Universal/PCD/Pei/Service.c
+++ b/EdkModulePkg/Universal/PCD/Pei/Service.c
@@ -62,10 +62,12 @@ PeiRegisterCallBackWorker (
   ASSERT (GuidHob != NULL);
   
   CallbackTable = GET_GUID_HOB_DATA (GuidHob);
+  CallbackTable = CallbackTable + (TokenNumber * FixedPcdGet32(PcdMaxPeiPcdCallBackNumberPerPcdEntry));
 
   Compare = Register? NULL: CallBackFunction;
   Assign  = Register? CallBackFunction: NULL;
 
+
   for (Idx = 0; Idx < FixedPcdGet32(PcdMaxPeiPcdCallBackNumberPerPcdEntry); Idx++) {
     if (CallbackTable[Idx] == Compare) {
       CallbackTable[Idx] = Assign;
diff --git a/EdkModulePkg/Universal/PCD/Pei/Service.h b/EdkModulePkg/Universal/PCD/Pei/Service.h
index 611427b578..b0f066817c 100644
--- a/EdkModulePkg/Universal/PCD/Pei/Service.h
+++ b/EdkModulePkg/Universal/PCD/Pei/Service.h
@@ -42,43 +42,43 @@ GetPcdDatabase (
 
 EFI_STATUS
 SetWorker (
-  IN UINTN         TokenNumber,
+  IN PCD_TOKEN_NUMBER   TokenNumber,
+  IN VOID               *Data,
+  IN UINTN              Size,
+  IN BOOLEAN            PtrType
+  )
+;
+
+EFI_STATUS
+SetWorkerByLocalTokenNumber (
+  IN UINT32        LocalTokenNumber,
   IN VOID          *Data,
   IN UINTN         Size,
   IN BOOLEAN       PtrType
   )
 ;
 
-EFI_STATUS
-SetWorkerByLocalTokenNumber (
-  UINT32        LocalTokenNumber,
-  VOID          *Data,
-  UINTN         Size,
-  BOOLEAN       PtrType
-  )
-;
-
 EFI_STATUS
 ExSetWorker (
-  IN UINT32               ExTokenNumber,
+  IN PCD_TOKEN_NUMBER     ExTokenNumber,
   IN CONST EFI_GUID       *Guid,
-  VOID                    *Data,
-  UINTN                   Size,
-  BOOLEAN                 PtrType
+  IN VOID                 *Data,
+  IN UINTN                Size,
+  IN BOOLEAN              PtrType
   )
 ;
 
 VOID *
 GetWorker (
-  UINTN     TokenNumber,
-  UINTN     GetSize
+  IN PCD_TOKEN_NUMBER     TokenNumber,
+  IN UINTN                GetSize
   )
 ;
 
 VOID *
 ExGetWorker (
   IN CONST EFI_GUID   *Guid,
-  IN UINT32           ExTokenNumber,
+  IN PCD_TOKEN_NUMBER ExTokenNumber,
   IN UINTN            GetSize
   )
 ;
@@ -92,19 +92,25 @@ typedef struct {
 VOID
 GetExPcdTokenAttributes (
   IN CONST EFI_GUID             *Guid,
-  IN UINT32                     ExTokenNumber,
+  IN PCD_TOKEN_NUMBER           ExTokenNumber,
   OUT EX_PCD_ENTRY_ATTRIBUTE    *ExAttr
   )
 ;
 
+
+
+
 EFI_STATUS
 PeiRegisterCallBackWorker (
-  IN  UINTN              TokenNumber,
+  IN  PCD_TOKEN_NUMBER   TokenNumber,
   IN  CONST GUID         *Guid, OPTIONAL
   IN  PCD_PPI_CALLBACK   CallBackFunction,
   IN  BOOLEAN            Register
 );
 
+
+
+
 VOID
 BuildPcdDatabase (
   VOID
diff --git a/EdkModulePkg/Universal/PCD/Test/PcdTest.c b/EdkModulePkg/Universal/PCD/Test/PcdTest.c
index 21fa67a6aa..6f2a5f1f1f 100644
--- a/EdkModulePkg/Universal/PCD/Test/PcdTest.c
+++ b/EdkModulePkg/Universal/PCD/Test/PcdTest.c
@@ -23,9 +23,10 @@ CONST GUID Guid1  = GUID1;
 EFI_STATUS
 EFIAPI
 OnsetCallback1 (
-  IN  UINT32    CallBackToken,
-  IN  VOID      *TokenData,
-  IN  UINTN     TokenDataSize
+  IN  CONST EFI_GUID  *Guid,
+  IN  UINT32          CallBackToken,
+  IN  VOID            *TokenData,
+  IN  UINTN           TokenDataSize
   )
 {
   DebugPrint (0x80000000, "In CallbackOnSet %x %d\n", * ((UINT32 *)TokenData), TokenDataSize);    
@@ -42,6 +43,10 @@ DoTest(
   UINT16    u16;
   UINT32    u32;
   UINT64    u64;
+  PCD_TOKEN_NUMBER Token;
+  
+
+  LibPcdCallbackOnSet (NULL, PcdToken(PcdTestDynamicUint32), OnsetCallback1);
   
   u32 = 0xafafafaf;
   PcdSet32(PcdTestDynamicUint32, u32);
@@ -57,6 +62,15 @@ DoTest(
   ASSERT (u16 == 0x1234);
   ASSERT (u64 == PcdGet64(PcdTestDynamicUint64));
   ASSERT (u32 == PcdGet32(PcdTestDynamicUint32));
+
+
+  Token = PCD_INVALID_TOKEN_NUMBER;
+
+  do {
+    Token = LibPcdGetNextToken (NULL, Token);
+    DebugPrint (EFI_D_ERROR, "Next Token Number is %d\n", Token);
+  } while (Token != PCD_INVALID_TOKEN_NUMBER);
+  
   
   return;
 }