// Too hard - details depend on target too much
//
USBDM_ErrorCode HCS12Unsecure::unsecureHCS12Target(void) {
   unsigned long speed;
   USBDMStatus_t usbdmStatus;

   print("HCS12Unsecure::unsecureHCS12Target()\n");

   USBDM_GetBDMStatus(&usbdmStatus);
   USBDM_GetSpeed(&speed);

   // Confirm speed
   HCS12Unsecure dialogue(false, speed);
   dialogue.Create(NULL);
   int getCancelOK = dialogue.ShowModal();

   if (getCancelOK != wxID_OK)
      return BDM_RC_FAIL;
   USBDM_SetSpeed(speed);
   dialogue.bulkEraseMemory();
   unsigned long bdmscr;
   USBDM_ReadStatusReg(&bdmscr); // for debug = UNSEC should be set
   dialogue.programSecurityLocation();
//   closeLogFile();

   return BDM_RC_OK;
}
//! (HCS12) Reads one byte from the BDM address space
//!
//! @param address 16-bit memory address
//! @return 8-bit data value
//!
//! @note Access to BDMSTS register address is mapped to USBDM_ReadStatusReg()
//!
TBDML_API unsigned char _tbdml_read_bd(unsigned int address) {
uint8_t rc;
unsigned long value;

   if (address == HC12_BDMSTS)
      rc = USBDM_ReadStatusReg(&value);
   else
      rc = USBDM_ReadDReg(address, &value);

   print("_tbdml_read_bd(%4.4X) => %2.2X\n", address, value);

   if (rc != BDM_RC_OK)
      return 0;

	return (uint8_t)value;
}
//!  2.2.8.6 Get DI Execution/Exit Status
//!
//! @param pdesExitStatus
//!
//! @return \n
//!     DI_OK              => OK \n
//!     DI_ERR_FATAL       => Error see \ref currentErrorString
//!
USBDM_GDI_DECLSPEC
DiReturnT DiExecGetStatus ( pDiExitStatusT pdesExitStatus ) {
   LOGGING;

   USBDM_ErrorCode      BDMrc;
   static DiExitCauseT  lastStatus     = DI_WAIT_USER;

   // Defaults
   pdesExitStatus->dscCause = DI_WAIT_UNKNOWN;
   pdesExitStatus->dwBpId   = 0; // bkpt ID?
   pdesExitStatus->szReason = (DiStringT)"unknown state";
   if (bdmOptions.autoReconnect) {
      USBDM_ErrorCode bdmRc = bdmInterface->targetConnectWithRetry(softConnectOptions);
      if (bdmRc != BDM_RC_OK) {
         log.print("=> DI_ERR_COMMUNICATION\n");
         return setErrorState(DI_ERR_COMMUNICATION, bdmRc);
      }
   }
   log.print("Calling USBDM_GetBDMStatus()\n");
   USBDMStatus_t USBDMStatus;
   USBDM_GetBDMStatus(&USBDMStatus);
//   pdesExitStatus->szReason = (DiStringT)getBDMStatusName(&USBDMStatus);
   if (USBDMStatus.connection_state == SPEED_NO_INFO) {
      log.print("=> NO_INFO\n");
      return setErrorState(DI_OK);
//      log.print("DiExecGetStatus()=>DI_ERR_COMMUNICATION\n");
//      return setErrorState(DI_ERR_NONFATAL, "Connection with target lost");
   }
   if (USBDMStatus.reset_recent == RESET_DETECTED) {
      log.print("=> Target has been reset\n");
      mtwksDisplayLine("Target RESET detected\n");
   }
   unsigned long status;
   BDMrc = USBDM_ReadStatusReg(&status);
   if (BDMrc != BDM_RC_OK) {
      log.print("=> Status read failed\n");
      return setErrorState(DI_OK);
//      return setErrorState(DI_ERR_NONFATAL, BDMrc);
   }
   if ((status&CFV1_XCSR_ENBDM) == 0) {
      log.print("=> ENBDM=0\n");
      return setErrorState(DI_OK);
//      log.print("DiExecGetStatus()=>DI_ERR_NONFATAL\n");
//      return setErrorState(DI_ERR_NONFATAL, "Connection with target lost");
   }
   if ((status&CFV1_XCSR_STOP) != 0) {
      // Stopped - low power sleep, treated as running
//      pdesExitStatus->dscCause = DI_WAIT_EXTERNAL|DI_WAIT_MISCELLANEOUS;
      pdesExitStatus->dscCause = DI_WAIT_RUNNING;
      pdesExitStatus->szReason = (DiStringT)"Target Stopped (Low power)...";
      if (lastStatus != pdesExitStatus->dscCause) {
//         log.print("DiExecGetStatus() status change => DI_WAIT_EXTERNAL|DI_WAIT_MISCELLANEOUS, (%s)\n",
         log.print("DiExecGetStatus() status change => DI_WAIT_RUNNING, (%s)\n",
               pdesExitStatus->szReason);
      }
   }
   else if ((status&CFV1_XCSR_HALT) != 0) {
      // Halted - in debug halted mode
      pdesExitStatus->dscCause = DI_WAIT_MISCELLANEOUS;
      pdesExitStatus->szReason = (DiStringT)"Debug Halted";
      if (lastStatus != pdesExitStatus->dscCause) {
         log.print("Status change => DI_WAIT_MISCELLANEOUS, (%s)\n",
               pdesExitStatus->szReason);
#if (TARGET==CFV1) && defined(CONVERT_RESETS_TO_EXCEPTIONS)
         // Read-write PC on halt
         // This causes Illegal Operand and Address Errors to be converted from
         // Resets to Exceptions.
         // Without this code the debugger is a bit misleading as it halts at the
         // start of the exception handler but then does the reset on resume!
         unsigned long PCValue;
         USBDM_ReadCReg(CFV1_CRegPC, &PCValue);
         USBDM_WriteCReg(CFV1_CRegPC, PCValue);
#endif
      }
   }
   else {
      // Processor executing
      pdesExitStatus->dscCause = DI_WAIT_RUNNING;
      pdesExitStatus->szReason = (DiStringT)"Running";
      if (lastStatus != pdesExitStatus->dscCause) {
         log.print("Status change => DI_WAIT_RUNNING, (%s)\n",
               pdesExitStatus->szReason);
      }
   }
   log.print("Reason = %s\n", pdesExitStatus->szReason);
   lastStatus = pdesExitStatus->dscCause;
   return setErrorState(DI_OK);
}
//! 2.2.6.2 Read Value from Register
//!
//! @param dnRegNumber
//! @param drvValue
//!
USBDM_GDI_DECLSPEC
DiReturnT DiRegisterRead ( DiUInt32T         dnRegNumber,
                           pDiRegisterValueT drvValue ) {
   LOGGING;
   log.print("0x%X(%d)\n", dnRegNumber, dnRegNumber);

   unsigned long     dataValue = 0xDEADBEEF;
   USBDM_ErrorCode   rc        = BDM_RC_OK;
   log.print("0x%X(%d)\n", dnRegNumber, dnRegNumber);

   if (forceMassErase) {
      // Dummy register reads until device in unsecured
      *drvValue = (U32c)dataValue;
      return setErrorState(DI_OK);
   }
   CHECK_ERROR_STATE();

   if (dnRegNumber>cfv1regID_FIRST_DEBUG_regID_BYTE)
      switch (dnRegNumber) {
         case cfv1regID_xcsr_byte :
            rc = USBDM_ReadStatusReg(&dataValue);
            break;
         case cfv1regID_csr2_byte :
            rc = USBDM_ReadDReg(CFV1_DRegCSR2byte,&dataValue);
            break;
         case cfv1regID_csr3_byte :
            rc = USBDM_ReadDReg(CFV1_DRegCSR3byte,&dataValue);
            break;
         default                  :
            log.print("DiRegisterRead(Illegal Reg# 0x%X(%d)\n", dnRegNumber, dnRegNumber);
            rc = BDM_RC_ILLEGAL_PARAMS;
            break;
      }
   else if (dnRegNumber>cfv1regID_FIRST_DEBUG_REG)
      rc = USBDM_ReadDReg(dnRegNumber-cfv1regID_FIRST_DEBUG_REG,&dataValue);
   else if (dnRegNumber > cfv1regID_FIRST_CONTROL_REG)
      rc = USBDM_ReadCReg(dnRegNumber-cfv1regID_FIRST_CONTROL_REG,&dataValue);
   else {
      switch (dnRegNumber) {
         case cfv1regID_pc :
            rc = USBDM_ReadCReg(CFV1_CRegPC,&dataValue);
            break;
         case cfv1regID_sr :
            rc = USBDM_ReadCReg(CFV1_CRegSR,&dataValue);
            break;
         default : // D0-7, A0-7
            if (dnRegNumber>15) {
               log.print("DiRegisterRead(Illegal Reg# 0x%X(%d)\n", dnRegNumber, dnRegNumber);
               rc = BDM_RC_ILLEGAL_PARAMS;
            }
            else
               rc = USBDM_ReadReg(dnRegNumber,&dataValue);
            break;
      }
   }
   if (rc != BDM_RC_OK) {
      log.print("DiRegisterRead(0x%X) => error\n", dnRegNumber);
      return setErrorState(DI_ERR_NONFATAL, rc);
   }
   *drvValue = (U32c)dataValue;
   log.print("0x%lX(%ld) => 0x%08lX\n", (unsigned long)dnRegNumber, (unsigned long)dnRegNumber, (unsigned long)dataValue);
   return setErrorState(DI_OK);
}
//! \brief Does Bulk Erase of Target Flash.
//!
//! @return error code, see \ref FlashError_t
//!
//! @note The target is not reset so current security state persists after erase.
//!
USBDM_ErrorCode HCS12Unsecure::bulkEraseMemory() {
   const uint8_t allOnes        = 0xFF;
   const uint8_t allZeroes      = 0x00;
   uint8_t dummyFlashAddress[]  = {0xFF, 0xFE};
   uint8_t dummyEepromAddress[] = {0x0C, 0x00};
   uint8_t dummyFlashData[]     = {0xFF, 0xFF};
   int timeout;
   uint8_t statValue;
   USBDM_ErrorCode rc;

   print("HCS12Unsecure::bulkEraseMemory():Bulk erasing target...\n");

   // Erase chip
   //=============================

   // Set up flash & eeprom
   rc = initialiseTargetFlash();
   if (rc != BDM_RC_OK) {
      print("HCS12Unsecure::bulkEraseMemory(): initialiseTargetFlash() failed, reason=%s\n",
            USBDM_GetErrorString(rc));
      return rc;
   }

   print("HCS12Unsecure::bulkEraseMemory():Bulk erasing target Flash...\n");
   // Apply Bulk Erase operation to all Flash banks
   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFPROTAddress(),   &allOnes);
   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFSTATAddress(),   &HCS12_clearFlashErrors);
   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFTSTMODAddress(), &allZeroes);
   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFSTATAddress(),   &HCS12_clearFlashErrors);
   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFTSTMODAddress(), &HCS12_FTSTMOD_WRALL);
   USBDM_WriteMemory(2, 2, HCS12DeviceData::getFADDRAddress(),   dummyFlashAddress);
   USBDM_WriteMemory(2, 2, HCS12DeviceData::getFDATAAddress(),   dummyFlashData);
   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFCMDAddress(),    &mMassErase);
   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFSTATAddress(),   &startFlashCommand);

   // Wait for flash command to complete
   timeout = 10;
   do {
      // Mass erase should take ~100ms
      wxMilliSleep(100);
      if (USBDM_ReadMemory(1,1,HCS12DeviceData::getFSTATAddress(),&statValue) != BDM_RC_OK)
         return BDM_RC_FAIL;
      if (timeout-- == 0)
         return BDM_RC_FAIL;
   } while ((statValue & 0xC0) != 0xC0);

   USBDM_WriteMemory(1, 1, HCS12DeviceData::getFTSTMODAddress(), &allZeroes);

   if (hasEEPROM) {
      print("HCS12Unsecure::bulkEraseMemory():Bulk erasing target EEPROM...\n");
      // Apply Bulk Erase operation to EEPROM
      USBDM_WriteMemory(1, 1, HCS12DeviceData::getEPROTAddress(),   &allOnes);
      USBDM_WriteMemory(1, 1, HCS12DeviceData::getESTATAddress(),   &HCS12_clearFlashErrors);
      USBDM_WriteMemory(2, 2, HCS12DeviceData::getEADDRAddress(),   dummyEepromAddress);
      USBDM_WriteMemory(2, 2, HCS12DeviceData::getEDATAAddress(),   dummyFlashData);
      USBDM_WriteMemory(1, 1, HCS12DeviceData::getECMDAddress(),    &mMassErase);
      USBDM_WriteMemory(1, 1, HCS12DeviceData::getESTATAddress(),   &startFlashCommand);

      // Wait for flash command to complete
      timeout = 10;
      do {
         // Mass erase should take ~100ms
         wxMilliSleep(100);
         if (USBDM_ReadMemory(1,1,HCS12DeviceData::getESTATAddress(),&statValue) != BDM_RC_OK)
            return BDM_RC_FAIL;
         if (timeout-- == 0)
            return BDM_RC_FAIL;
      } while ((statValue & 0xC0) != 0xC0);
   }
   USBDM_TargetReset((TargetMode_t)(RESET_HARDWARE|RESET_SPECIAL));

   unsigned long usbdmStatus;
   USBDM_ReadStatusReg(&usbdmStatus);

   if ((usbdmStatus & HC12_BDMSTS_UNSEC) != 0) {
      print("HCS12Unsecure::bulkEraseMemory():Bulk erasing target memory...Complete\n");
      return BDM_RC_OK;
   }
   else {
      print("HCS12Unsecure::bulkEraseMemory():Bulk erasing target memory...Failed!\n");
      return BDM_RC_FAIL;
   }
}
//!  Determines the trim value for the target internal clock.
//!  The target clock is left trimmed for a bus freq. of targetBusFrequency.
//!
//!     Target clock has been suitably configured.
//!
//!  @param      trimAddress           Address of trim register.
//!  @param      targetBusFrequency    Target Bus Frequency to trim to.
//!  @param      returnTrimValue       Resulting trim value (9-bit number)
//!  @param      measuredBusFrequency  Resulting Bus Frequency
//!  @param      do9BitTrim            True to do 9-bit trim (rather than 8-bit)
//!
//!  @return
//!   == \ref PROGRAMMING_RC_OK  => Success \n
//!   != \ref PROGRAMMING_RC_OK  => Various errors
//!
USBDM_ErrorCode FlashProgrammer::trimTargetClock(uint32_t       trimAddress,
                                                 unsigned long  targetBusFrequency,
                                                 uint16_t      *returnTrimValue,
                                                 unsigned long *measuredBusFrequency,
                                                 int            do9BitTrim){
   LOGGING;
   uint8_t          mask;
   uint8_t          trimMSB, trimLSB, trimCheck;
   int              trimValue;
   int              maxRange;
   int              minRange;
   unsigned         long bdmSpeed;
   int              index;
   USBDM_ErrorCode  rc = PROGRAMMING_RC_OK;

#if TARGET == RS08
   mask = RS08_BDCSCR_CLKSW;
#elif TARGET == HCS08
   mask = HC08_BDCSCR_CLKSW;
#elif TARGET == HCS12
   mask = HC12_BDMSTS_CLKSW;
#elif TARGET == CFV1
   mask = CFV1_XCSR_CLKSW;
#endif

   unsigned long BDMStatusReg;
   rc = USBDM_ReadStatusReg(&BDMStatusReg);
   if ((BDMStatusReg&mask) == 0) {
      Logging::print("Setting CLKSW\n");
      BDMStatusReg |= mask;
#if TARGET == CFV1
      // Make sure we don't accidently do a mass erase
      mask &= ~CFV1_XCSR_ERASE;
#endif
      rc = USBDM_WriteControlReg(BDMStatusReg);
      rc = USBDM_Connect();
   }
   static const int maxTrim        = 505;   // Maximum acceptable trim value
   static const int minTrim        =   5;   // Minimum acceptable trim value
   static const int SearchOffset   =   8;   // Linear sweep range is +/- this value
   static const unsigned char zero =   0;
   const unsigned long targetBDMFrequency = targetBusFrequency/parameters.getBDMtoBUSFactor();
   int numAverage;     // Number of times to repeat measurements

   double sumX          = 0.0;
   double sumY          = 0.0;
   double sumXX         = 0.0;
   double sumYY         = 0.0;
   double sumXY         = 0.0;
   double num           = 0.0;
   double alpha, beta;
   double trimValueF;

   Logging::print("targetBusFrequency=%ld, targetBDMFrequency=%ld)\n", targetBusFrequency, targetBDMFrequency);

   flashReady = FALSE; // Not configured for Flash access

   // Set safe defaults
   *returnTrimValue      = 256;
   *measuredBusFrequency = 10000;
   trimMSB               = 0;

   // Set LSB trim value = 0
   if (writeClockRegister(trimAddress+1, zero) != BDM_RC_OK) {
      return PROGRAMMING_RC_ERROR_BDM_WRITE;
   }
   // Initial binary search (MSB only)
   for (mask = 0x80; mask > 0x0; mask>>=1) {
      trimMSB |= mask;
      // Set trim value (MSB only)
      if (writeClockRegister(trimAddress, trimMSB) != BDM_RC_OK) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      // Check target speed
      if (USBDM_Connect() != BDM_RC_OK) {
         return PROGRAMMING_RC_ERROR_BDM_CONNECT;
      }
      if (USBDM_GetSpeed(&bdmSpeed) != BDM_RC_OK) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      bdmSpeed *= 1000; // convert to Hz

    Logging::print("Binary search: trimMSB=0x%02X (%d), bdmSpeed=%ld%c\n",
            trimMSB, trimMSB, bdmSpeed, (bdmSpeed<targetBDMFrequency)?'-':'+');

      // Adjust trim value
      if (bdmSpeed<targetBDMFrequency) {
         trimMSB &= ~mask; // too slow
      }
      if (trimMSB > maxTrim/2) {
         trimMSB = maxTrim/2;
      }
      if (trimMSB < minTrim/2) {
         trimMSB = minTrim/2;
      }
   }

   // Binary search value is middle of range to sweep

   trimValue = trimMSB<<1;  // Convert to 9-bit value

   // Linear sweep +/-SEARCH_OFFSET, starting at higher freq (smaller Trim)
   // Range is constrained to [minTrim..maxTrim]

   maxRange = trimValue + SearchOffset;
   if (maxRange > maxTrim) {
      maxRange = maxTrim;
   }
   minRange = trimValue - SearchOffset;
   if (minRange < minTrim) {
      minRange = minTrim;
   }
//   Logging::print("trimTargetClock(): Linear sweep, f=%6ld    \n"
//                   "trimTargetClock():    Trim       frequency \n"
//                   "========================================== \n",
//                   targetBDMFrequency/1000);

   if (do9BitTrim) {
      numAverage = 2;
   }
   else {
      numAverage = 4;
   }
   for(trimValue=maxRange; trimValue>=minRange; trimValue--) {
      trimLSB = trimValue&0x01;
      trimMSB = (uint8_t)(trimValue>>1);
      if (do9BitTrim) {
         // Write trim LSB
         if (writeClockRegister(trimAddress+1, trimLSB) != BDM_RC_OK)
            return PROGRAMMING_RC_ERROR_BDM_WRITE;
         if (USBDM_ReadMemory(1, 1, trimAddress+1,   &trimCheck) != BDM_RC_OK)
            return PROGRAMMING_RC_ERROR_BDM_WRITE;
         if ((trimCheck&0x01) != trimLSB)
            return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      else if (trimValue&0x01) {
         // skip odd trim values if 8-bit trim
         continue;
      }
      // Write trim MSB
      if (writeClockRegister(trimAddress,  trimMSB) != BDM_RC_OK) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      if (USBDM_ReadMemory(1, 1, trimAddress,   &trimCheck) != BDM_RC_OK) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      if (trimCheck != trimMSB) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      //milliSleep(100);
      // Measure sync multiple times
      for(index=numAverage; index>0; index--) {
         // Check target speed
         if (USBDM_Connect() != BDM_RC_OK)
            return PROGRAMMING_RC_ERROR_BDM_CONNECT;
         if (USBDM_GetSpeedHz(&bdmSpeed) != BDM_RC_OK)
            return PROGRAMMING_RC_ERROR_BDM_CONNECT;
         sumX  += trimValue;
         sumY  += bdmSpeed;
         sumXX += trimValue*trimValue;
         sumYY += bdmSpeed*bdmSpeed;
         sumXY += bdmSpeed*trimValue;
         num   += 1.0;
//         Logging::print("trimTargetClock(): %6d    %10ld %10ld\n", trimValue, bdmSpeed, targetBDMFrequency-bdmSpeed);
      }
   }
   for(trimValue=minRange; trimValue<=maxRange; trimValue++) {
      trimLSB = trimValue&0x01;
      trimMSB = (uint8_t)(trimValue>>1);
      if (do9BitTrim) {
         // Write trim LSB
         if (writeClockRegister(trimAddress+1, trimLSB) != BDM_RC_OK) {
            return PROGRAMMING_RC_ERROR_BDM_WRITE;
         }
         if (USBDM_ReadMemory(1, 1, trimAddress+1,   &trimCheck) != BDM_RC_OK) {
            return PROGRAMMING_RC_ERROR_BDM_WRITE;
         }
         if ((trimCheck&0x01) != trimLSB) {
            return PROGRAMMING_RC_ERROR_BDM_WRITE;
         }
      }
      else if (trimValue&0x01) {
         // skip odd trim values if 8-bit trim
         continue;
      }
      // Write trim MSB
      if (writeClockRegister(trimAddress, trimMSB) != BDM_RC_OK) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      if (USBDM_ReadMemory(1, 1, trimAddress,   &trimCheck) != BDM_RC_OK) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      if (trimCheck != trimMSB) {
         return PROGRAMMING_RC_ERROR_BDM_WRITE;
      }
      //milliSleep(100);
      // Measure sync multiple times
      for(index=numAverage; index>0; index--) {
         // Check target speed
         if (USBDM_Connect() != BDM_RC_OK) {
            return PROGRAMMING_RC_ERROR_BDM_CONNECT;
         }
         if (USBDM_GetSpeedHz(&bdmSpeed) != BDM_RC_OK) {
            return PROGRAMMING_RC_ERROR_BDM_CONNECT;
         }
         sumX  += trimValue;
         sumY  += bdmSpeed;
         sumXX += trimValue*trimValue;
         sumYY += bdmSpeed*bdmSpeed;
         sumXY += bdmSpeed*trimValue;
         num   += 1.0;
//         Logging::print("trimTargetClock(): %6d    %10ld %10ld\n", trimValue, bdmSpeed, targetBDMFrequency-bdmSpeed);
      }
   }

//   Logging::print("N=%f, sumX=%f, sumXX=%f, sumY=%f, sumYY=%f, sumXY=%f\n",
//                    num, sumX, sumXX, sumY, sumYY, sumXY);

   // Calculate linear regression co-efficients
   beta  = (num*sumXY-sumX*sumY)/(num*sumXX-sumX*sumX);
   alpha = (sumY-beta*sumX)/num;

   // Estimate required trim value
   trimValueF = ((targetBDMFrequency-alpha)/beta);

   if ((trimValueF <= 5.0) || (trimValue >= 505.0)) { // resulted in extreme value
      trimValueF = 256.0;                             // replace with 'Safe' trim value
      rc = PROGRAMMING_RC_ERROR_TRIM;
   }

   trimValue = (int)round(trimValueF);
   trimMSB   = trimValue>>1;
   trimLSB   = trimValue&0x01;

//   Logging::print("alpha= %f, beta= %f, trimF= %f, trimMSB= %d (0x%02X), trimLSB= %d\n",
//           alpha, beta, trimValueF, trimMSB, trimMSB, trimLSB);

//   Logging::print("trimTargetClock() Result: trim=0x%3.3x (%d), measured bdmSpeed=%ld\n",
//           savedTrimValue, savedTrimValue, bestFrequency);

   *returnTrimValue  = trimValue;

   // Set trim value (LSB first)
   if ((do9BitTrim && (writeClockRegister(trimAddress+1, trimLSB) != BDM_RC_OK)) ||
       (writeClockRegister(trimAddress, trimMSB) != BDM_RC_OK)) {
      return PROGRAMMING_RC_ERROR_BDM_WRITE;
   }
   // Check connection at that speed
   if (USBDM_Connect() != BDM_RC_OK) {
      return PROGRAMMING_RC_ERROR_BDM_CONNECT;
   }
   if (USBDM_GetSpeedHz(&bdmSpeed) != BDM_RC_OK) {
      return PROGRAMMING_RC_ERROR_BDM_CONNECT;
   }
   *measuredBusFrequency = bdmSpeed*parameters.getBDMtoBUSFactor();

   return rc;
}
//! (CFv1) Read XCSR.msb
//!
//! @return 8-bit number
//!
OSBDM_API unsigned int  _opensourcebdm_read_xcsr_byte(void) {
unsigned long BDMStatus;

   USBDM_ReadStatusReg(&BDMStatus);
   return (U8) BDMStatus;
}
//! (HCS12, HCS08 & RS08) Read target BDM status register (BDCSC, BDMSTS or XCSR as appropriate)
//!
//! @return 8-bit value
//!
//!
OSBDM_API unsigned char _opensourcebdm_read_status(void){
unsigned long BDMStatus = 0;

   USBDM_ReadStatusReg(&BDMStatus);
   return (U8)BDMStatus;
}