BOOL
CRomiDisk::WaitForInterrupt(
DWORD dwTimeOut
)
{
BYTE bStatus;
BOOL fRet = TRUE;
DWORD dwRet;
// wait for interrupt
dwRet = WaitForSingleObject(m_pPort->m_hIRQEvent, dwTimeOut);
if (dwRet == WAIT_TIMEOUT) {
fRet = FALSE;
}
else {
if (dwRet != WAIT_OBJECT_0) {
if (!WaitForDisc(WAIT_TYPE_DRQ, dwTimeOut, 10)) {
fRet = FALSE;
}
}
}
// read status; acknowledge interrupt
bStatus = GetBaseStatus();
if (bStatus & ATA_STATUS_ERROR) {
bStatus = GetError();
fRet = FALSE;
}
// signal interrupt done
InterruptDone(m_pPort->m_dwSysIntr);
return fRet;
}
void
CPCIDisk::EnableInterrupt(
)
{
GetBaseStatus(); // acknowledge interrupt, if pending
// signal interrupt done
InterruptDone(m_pPort->m_dwSysIntr);
}
void M3LedX2XNEcShm::SetCommandFromSds(unsigned char * data)
{
M3LedX2XNEcShmSdsCommand * sds = (M3LedX2XNEcShmSdsCommand *) data;
request_command();
memcpy(&command_from_sds, sds, GetCommandSdsSize());
release_command();
int64_t dt = GetBaseStatus()->timestamp()-command_from_sds.timestamp; // microseconds
bool shm_timeout = ABS(dt) > (timeout*1000);
/* if (tmp_cnt++ == 200)
{
M3_DEBUG("hi\n");
tmp_cnt = 0;
}*/
if (led_x2xn != NULL)
{
if (shm_timeout)
{
/* if (tmp_cnt++ == 200)
{
M3_DEBUG("hi\n");
tmp_cnt = 0;
}*/
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_a(0);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_b(0);
} else {
/* if (tmp_cnt++ == 200)
{
M3_DEBUG("here\n");
tmp_cnt = 0;
}*/
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_a(command_from_sds.enable_a);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_b(command_from_sds.enable_b);
for (int i = 0; i < NUM_PER_BRANCH; i++)
{
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_a()->set_r(i,command_from_sds.r_a[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_a()->set_g(i,command_from_sds.g_a[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_a()->set_b(i,command_from_sds.b_a[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_b()->set_r(i,command_from_sds.r_b[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_b()->set_g(i,command_from_sds.g_b[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_b()->set_b(i,command_from_sds.b_b[i]);
}
}
}
}
void M3LedX2XNEcShm::SetSdsFromStatus(unsigned char * data)
{
status_to_sds.timestamp = GetBaseStatus()->timestamp();
M3LedX2XNEcShmSdsStatus * sds = (M3LedX2XNEcShmSdsStatus *) data;
request_status();
memcpy(sds, &status_to_sds, GetStatusSdsSize());
release_status();
}
void M3TorqueShm::SetCommandFromSds(unsigned char * data)
{
M3TorqueShmSdsCommand * sds = (M3TorqueShmSdsCommand *) data;
request_command();
memcpy(&command_from_sds, sds, GetCommandSdsSize());
release_command();
command.set_timestamp(command_from_sds.timestamp);
int64_t dt = GetBaseStatus()->timestamp()-command.timestamp(); // microseconds
bool shm_timeout = ABS(dt) > (timeout*1000);
for (int i = 0; i < bot->GetNdof(RIGHT_ARM); i++)
{
command.mutable_right_arm()->set_tq_desired(i, command_from_sds.right_arm.tq_desired[i]);
if (shm_timeout)
{
bot->DisableTorqueShmRightArm();
} else{
bot->EnableTorqueShmRightArm();
bot->SetTorqueSharedMem_mNm(RIGHT_ARM, i, command.right_arm().tq_desired(i));
}
}
for (int i = 0; i < bot->GetNdof(LEFT_ARM); i++)
{
command.mutable_left_arm()->set_tq_desired(i, command_from_sds.left_arm.tq_desired[i]);
if (shm_timeout)
{
bot->DisableTorqueShmLeftArm();
} else{
bot->EnableTorqueShmLeftArm();
bot->SetTorqueSharedMem_mNm(LEFT_ARM, i, command.left_arm().tq_desired(i));
}
}
for (int i = 0; i < bot->GetNdof(TORSO); i++)
{
command.mutable_torso()->set_tq_desired(i, command_from_sds.torso.tq_desired[i]);
if (shm_timeout)
{
bot->DisableTorqueShmTorso();
} else{
bot->EnableTorqueShmTorso();
bot->SetTorqueSharedMem_mNm(TORSO, i, command.torso().tq_desired(i));
}
}
}
BOOL
CST202T_SATA::SetLookAhead(
)
{
BYTE bError = 0;
BYTE bStatus = 0;
// select device
SelectDevice();
// wait for device to acknowledge selection
WaitForDisc(WAIT_TYPE_NOT_BUSY, 100);
WaitForDisc(WAIT_TYPE_READY, 1000);
WaitOnBusy(TRUE);
// write command
WriteFeature(ATA_ENABLE_LOOKAHEAD);
WriteCommand(ATAPI_CMD_SET_FEATURES);
// wait for device to respond to command
WaitOnBusy(TRUE);
WaitForDisc(WAIT_TYPE_NOT_BUSY, 200);
// check response
bStatus = GetBaseStatus();
bError = GetError();
if ((bStatus & ATA_STATUS_ERROR) && (bError & ATA_ERROR_ABORTED)) {
DEBUGMSG(ZONE_ERROR, (_T(
"Atapi!CDisk::SetLookAhead> Failed to enable read look-ahead; status(%02X), error(%02X)\r\n"
), bStatus, bError));
ResetController(FALSE);
return FALSE;
}
return TRUE;
}
void M3TorqueShm::SetSdsFromStatus(unsigned char * data)
{
status_to_sds.timestamp = GetBaseStatus()->timestamp();
for (int i = 0; i < bot->GetNdof(RIGHT_ARM); i++)
{
status.mutable_right_arm()->set_theta(i, bot->GetThetaDeg(RIGHT_ARM,i));
status.mutable_right_arm()->set_thetadot(i, bot->GetThetaDotDeg(RIGHT_ARM,i));
status.mutable_right_arm()->set_torque(i, bot->GetTorque_mNm(RIGHT_ARM,i));
status.mutable_right_arm()->set_ctrl_mode(i, bot->GetMode(RIGHT_ARM,i));
status_to_sds.right_arm.theta[i] = status.right_arm().theta(i);
status_to_sds.right_arm.thetadot[i] = status.right_arm().thetadot(i);
status_to_sds.right_arm.torque[i] = status.right_arm().torque(i);
status_to_sds.right_arm.ctrl_mode[i] = status.right_arm().ctrl_mode(i);
}
if (right_loadx6)
{
for (int i = 0; i < 6; i++)
{
status.mutable_right_arm()->set_loadx6(i, right_loadx6->GetWrench(i));
status_to_sds.right_arm.wrench[i] = right_loadx6->GetWrench(i);
}
}
for (int i = 0; i < bot->GetNdof(LEFT_ARM); i++)
{
status.mutable_left_arm()->set_theta(i, bot->GetThetaDeg(LEFT_ARM,i));
status.mutable_left_arm()->set_thetadot(i, bot->GetThetaDotDeg(LEFT_ARM,i));
status.mutable_left_arm()->set_torque(i, bot->GetTorque_mNm(LEFT_ARM,i));
status.mutable_left_arm()->set_ctrl_mode(i, bot->GetMode(LEFT_ARM,i));
status_to_sds.left_arm.theta[i] = status.left_arm().theta(i);
status_to_sds.left_arm.thetadot[i] = status.left_arm().thetadot(i);
status_to_sds.left_arm.torque[i] = status.left_arm().torque(i);
status_to_sds.left_arm.ctrl_mode[i] = status.left_arm().ctrl_mode(i);
}
if (left_loadx6)
{
for (int i = 0; i < 6; i++)
{
status.mutable_left_arm()->set_loadx6(i, left_loadx6->GetWrench(i));
status_to_sds.left_arm.wrench[i] = left_loadx6->GetWrench(i);
}
}
for (int i = 0; i < bot->GetNdof(TORSO); i++)
{
status.mutable_torso()->set_theta(i, bot->GetThetaDeg(TORSO,i));
status.mutable_torso()->set_thetadot(i, bot->GetThetaDotDeg(TORSO,i));
status.mutable_torso()->set_torque(i, bot->GetTorque_mNm(TORSO,i));
status.mutable_torso()->set_ctrl_mode(i, bot->GetMode(TORSO,i));
status_to_sds.torso.theta[i] = status.torso().theta(i);
status_to_sds.torso.thetadot[i] = status.torso().thetadot(i);
status_to_sds.torso.torque[i] = status.torso().torque(i);
status_to_sds.torso.ctrl_mode[i] = status.torso().ctrl_mode(i);
}
for (int i = 0; i < bot->GetNdof(HEAD); i++)
{
status.mutable_head()->set_theta(i, bot->GetThetaDeg(HEAD,i));
status.mutable_head()->set_thetadot(i, bot->GetThetaDotDeg(HEAD,i));
status.mutable_head()->set_torque(i, bot->GetTorque_mNm(HEAD,i));
status.mutable_head()->set_ctrl_mode(i, bot->GetMode(HEAD,i));
status_to_sds.head.theta[i] = status.head().theta(i);
status_to_sds.head.thetadot[i] = status.head().thetadot(i);
status_to_sds.head.torque[i] = status.head().torque(i);
status_to_sds.head.ctrl_mode[i] = status.head().ctrl_mode(i);
}
M3TorqueShmSdsStatus * sds = (M3TorqueShmSdsStatus *) data;
request_status();
memcpy(sds, &status_to_sds, GetStatusSdsSize());
release_status();
}
BOOL
CST202T_SATA::ResetController(
BOOL bSoftReset // ignore
)
{
DWORD dwAttempts = 0;
DWORD dwStatus = 0;
BYTE bStatus = 0;
BOOL fRet = FALSE;
ToggleOOB(SATA_SCONTROL_SPD_1_5_SPEEDLIMIT); // This part supports generation 1 speeds only.
// SError will reflect several transient error conditions resulting from
// the reset sequence. Clear them now.
m_pRegSATA->dwSError = m_pRegSATA->dwSError;
// we have to negate the RESET signal for 5 microseconds before we assert it
WriteAltDriveController(0x00);
Sleep(2);
::StallExecution(25);
// Set_SRST
// --------
// to enter Set_SRST state, set SRST in the Device Control register to one;
// this will assert the RESET signal and reset both devices on the current
// channel
WriteAltDriveController(0x04); // 0x04 == SRST
// remain in this state for at least 5 microseconds; i.e., assert RESET signal
// for at least 5 microseconds
// if this is a hardware reset, then assert RESET signal for at least 25
// microseconds
Sleep(2);
::StallExecution(25); // this should be CEDDK implementation
// Clear_wait
// ----------
// clear SRST in the Device Control register, i.e., negate RESET signal
WriteAltDriveController(0x00);
// remain in this state for at least 2 milliseconds
Sleep(5);
HSR2_Check_status:;
// Check_status
// ------------
// read the Status or Alternate Status register
// if BSY is set to one, then re-enter this state
// if BSY is cleared to zero, check the ending status in the Error register
// and the signature (9.12) and transition to Host_Idle
bStatus = GetAltStatus(); // read Status register
if (bStatus & 0x80) {
// BSY is set to one, re-enter this state
if ( !((m_pPort->m_pController->m_pIdeReg->dwSoftResetTimeout - dwAttempts) % 100) )
{
DEBUGMSG(ZONE_INIT, (TEXT(
"Atapi!CDisk::ResetController> Device is busy; %u seconds remaining\r\n"
), ((m_pPort->m_pController->m_pIdeReg->dwSoftResetTimeout - dwAttempts)/100)));
}
Sleep(10);
dwAttempts += 1;
// a device has at most 31 seconds to complete a software reset; we'll use 3 seconds
if (dwAttempts == m_pPort->m_pController->m_pIdeReg->dwSoftResetTimeout) {
DEBUGMSG(ZONE_INIT, (TEXT("Atapi!CDisk::ResetController> Timeout\r\n")));
goto exit;
}
goto HSR2_Check_status;
}
DEBUGMSG(ZONE_INIT, (TEXT(
"Atapi!CDisk::ResetController> Device is ready\r\n"
)));
// BSY is cleared to zero, check the ending status in the Error register
// and the signature
// TODO: Check the signature (9.12)
// From SATA 1.0A, seciton 9.1, 9.3, and 9.4:
// Bit 0 of Error register is set to 1 if the reset succeeded.
BYTE bError;
bError = GetError(); // read Error register
bStatus = GetAltStatus(); // read Status register
if (!(bError & 0x01)) {
DEBUGMSG(ZONE_INIT, (TEXT(
"Atapi!CDisk::ResetController> SRST failed. Error = 0x%x, Status = 0x%x\r\n"
), bError, bStatus));
// TODO: Recover from error
goto exit;
}
fRet = TRUE;
exit:;
// SError will reflect several transient error conditions resulting from
// the reset sequence. Clear them now.
m_pRegSATA->dwSError = m_pRegSATA->dwSError;
// Clear any pending interrupt
bStatus = GetBaseStatus();
// Enable the SATA error interrupt (this is cleared by the COMRESET sequence).
EnableSErrorInt(TRUE);
return fRet;
}
DWORD CST202T_SATA::InterruptThread(
IN PVOID lParam
)
{
CST202T_SATA *pDisk = (CST202T_SATA *)lParam;
while(TRUE) {
WaitForSingleObject(pDisk->m_pPort->m_hIRQEvent, INFINITE);
// We have three interrupts of interest: the ATA interrupt, the
// DMA block transfer complete interrupt, and the SATA error
// interrupt.
//
// NOTE: We have no indicatior of the ATA interrupt being active.
// Thus, we must assume the ATA interrupt is the interrupt
// being triggered if none of the other interrupts is
// detected. Note that some versions of the SATA host
// document provided by ST describe bit 31 of INTPR
// as the "IPF" bit, which would indicate the state
// of the ATA interrupt. However, this bit is not
// present in this part.
////////////////////////////////////////////////////////////////
// First, determine if a SATA error condition has been detected.
if (IsSErrorIntActive())
{
// Simply acknowledge the interrupt here to unblock any pending
// read or write operation. The GetSATAError function will
// handle the SATA error at the appropriate time and re-enable
// the interrupt.
EnableSErrorInt(FALSE);
// Get ATA status and clear pending ATA interrupt condition.
pDisk->m_pPort->m_bStatus = GetBaseStatus();
// DEBUGMSG(ZONE_IO, (_T(
// "Atapi!CST202T_SATA::InterruptThread> Got SError Interrupt!\r\n"
// )));
SetEvent(pDisk->m_pPort->m_hErrorEvent);
}
else if (IsDMACTfrIntActive())
{
// If we're operating in DMA mode, the DMA controller may have
// generated the interrupt. The only DMA interrupt we are interested
// in is the transfer complete interrupt. ACK it here.
ClearDMACTfrInt();
// Get ATA status and clear pending ATA interrupt condition.
pDisk->m_pPort->m_bStatus = GetBaseStatus();
// DEBUGMSG(ZONE_IO, (_T(
// "Atapi!CST202T_SATA::InterruptThread> Got DMA Interrupt!\r\n"
// )));
SetEvent(pDisk->m_pPort->m_hDMAEvent);
}
else // ATA interrupt
{
// SATA interrupt condition.
pDisk->m_pPort->m_bStatus = GetBaseStatus();
// DEBUGMSG(ZONE_IO, (_T(
// "Atapi!CST202T_SATA::InterruptThread> Got SATA Interrupt!\r\n"
// )));
SetEvent(pDisk->m_pPort->m_hSATAEvent);
}
InterruptDone(pDisk->m_pPort->m_dwSysIntr);
}
return(0);
}