int DarwinController::SetMoveSpeed(unsigned char joint_ID, int move_speed){
unsigned char speed_packet[9] = {0, };
unsigned char params[2] = {GetLowByte(move_speed), GetHighByte(move_speed)};
MakePacket(speed_packet, joint_ID, 2, WRITE, GOAL_POSITION_L, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(speed_packet, rxpacket);
}
void Settings::ReadWrite(const Settings::Action action, QObject *qobject_p,
const QStringList &properties, const QStringList &ignore_objects)
{
const QPointer<QObject> qo_qp(qobject_p);
if (qo_qp) {
const auto object_name(qobject_p->objectName());
if (TRUE != object_name.isEmpty()) {
if (TRUE != ignore_objects.contains(object_name)) {
beginGroup(object_name);
SingleObjectPropertiesReadWrite(action, qobject_p, properties);
foreach (auto o_p, qobject_p->children()) {
QPointer<QObject> child_qo_qp(o_p);
if (child_qo_qp) {
const auto object_name(o_p->objectName());
if (TRUE != object_name.isEmpty()) {
if (TRUE != ignore_objects.contains(object_name)) {
ReadWrite(action, o_p, properties, ignore_objects);
}
}
}
}
endGroup();
}
}
}
int DarwinController::Set_PID_Gain(unsigned char joint_ID, unsigned char P_Value, unsigned char I_Value, unsigned char D_Value){
unsigned char PID_packet[10] = {0, };
unsigned char params[3] = {P_Value, I_Value, D_Value};
MakePacket(PID_packet, joint_ID, 1, WRITE, D_GAIN, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(PID_packet, rxpacket);
}
// Converts an angle given in degrees into motor ticks and sends write packet to motor
int DarwinController::SetJointAngle(unsigned char joint_ID, int goal_angle){
unsigned char angle_packet[9] = {0, };
int goal_ticks = goal_angle * 11.378; // 4096/360
unsigned char params[2] = {GetLowByte(goal_ticks), GetHighByte(goal_ticks)};
MakePacket(angle_packet, joint_ID, 2, WRITE, GOAL_POSITION_L, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(angle_packet, rxpacket);
}
NTSTATUS StartIrpWrite(
IN PC0C_IO_PORT pIoPort,
IN PLIST_ENTRY pQueueToComplete)
{
return ReadWrite(
pIoPort->pIoPortRemote, FALSE,
pIoPort, TRUE,
pQueueToComplete);
}
VOID TimeoutRoutine(
PC0C_IO_PORT pIoPort,
IN PC0C_IRP_QUEUE pQueue)
{
LIST_ENTRY queueToComplete;
KIRQL oldIrql;
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPort->pIoLock, &oldIrql);
if (pQueue->pCurrent) {
PC0C_IRP_STATE pState;
pState = GetIrpState(pQueue->pCurrent);
HALT_UNLESS(pState);
pState->flags |= C0C_IRP_FLAG_EXPIRED;
switch (pState->iQueue) {
case C0C_QUEUE_WRITE:
ReadWrite(pIoPort->pIoPortRemote, FALSE, pIoPort, FALSE, &queueToComplete);
break;
case C0C_QUEUE_READ:
ReadWrite(pIoPort, FALSE, pIoPort->pIoPortRemote, FALSE, &queueToComplete);
break;
case C0C_QUEUE_CLOSE:
FdoPortIo(C0C_IO_TYPE_CLOSE_COMPLETE,
NULL,
pIoPort,
&pIoPort->irpQueues[C0C_QUEUE_CLOSE],
&queueToComplete);
break;
}
}
KeReleaseSpinLock(pIoPort->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
}
/* ReadJointAngle - sends out a single read to a joint motor
* Return - joint angle in ticks of prompted joint
* - for failed read, return -99999
*/
int DarwinController::ReadJointAngle(int id){
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
unsigned char txpacketread[] = {0, 0, id, 0x04, 0x02, 0x24, 0x02, 0};
FinishPacket(txpacketread);
int result = ReadWrite(txpacketread, rxpacket);
if(result == 0){
printf("Failed read! \n");
return -9999999;
} else {
return MakeWord((int)rxpacket[PARAMETER], (int)rxpacket[PARAMETER + 1]);
}
}
/* Ping - send a ping to a specific id
* Return - true if the ping was successful
*/
bool DarwinController::Ping(int id, int *error){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = 1;
txpacket[LENGTH] = 2;
FinishPacket(txpacket);
int result = ReadWrite(txpacket, rxpacket);
if( result == 0){
return false;
} else {
return true;
}
}
VOID CancelRoutine(IN PDEVICE_OBJECT pDevObj, IN PIRP pIrp)
{
LIST_ENTRY queueToComplete;
PC0C_IO_PORT pIoPort;
PC0C_IRP_STATE pState;
KIRQL oldIrql;
PC0C_IRP_QUEUE pQueue;
IoReleaseCancelSpinLock(pIrp->CancelIrql);
pIoPort = FDO_PORT_TO_IO_PORT(pDevObj);
pState = GetIrpState(pIrp);
HALT_UNLESS(pState);
pQueue = &pIoPort->irpQueues[pState->iQueue];
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPort->pIoLock, &oldIrql);
if (pState->flags & C0C_IRP_FLAG_IN_QUEUE) {
RemoveEntryList(&pIrp->Tail.Overlay.ListEntry);
pState->flags &= ~C0C_IRP_FLAG_IN_QUEUE;
}
pIrp->IoStatus.Status = STATUS_CANCELLED;
InsertTailList(&queueToComplete, &pIrp->Tail.Overlay.ListEntry);
if (pState->flags & C0C_IRP_FLAG_IS_CURRENT) {
ShiftQueue(pQueue);
if (pState->iQueue == C0C_QUEUE_WRITE)
ReadWrite(pIoPort->pIoPortRemote, FALSE, pIoPort, FALSE, &queueToComplete);
}
KeReleaseSpinLock(pIoPort->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
}
/* LEER LINEAS DEL FICHERO*/
void
ReadLines(char* archivo, int fd_out){
FILE* fd_in;
char milinea[MAXLINEA];
char* linea=NULL;
int lon;
Comando* command;
int fp=0;
int is_primero=1;
//abrir el archivo
fd_in=fopen(archivo, "r");
if (fd_in==NULL){
err(1, "No se puede abrir el archivo %s\n", archivo);
//retornar error
}
//leer linea a linea
while(feof(fd_in)!=1){
linea=fgets(milinea, sizeof(milinea), fd_in);
if(linea!=NULL && strcmp(linea, "\n")!=0){ //lineas en blanco
lon=strlen(linea);
//quitar \n
if(linea[lon-1]=='\n'){
linea[lon-1]='\0';
}
//
command=(Comando*)malloc(512*sizeof(Comando));
fp=EjecutarLineas(linea, command, fp, fd_out, is_primero);
free(command);
is_primero=0;
}
}
ReadWrite(fp, fd_out);
close(fp);
close(fd_out);
//cerrar el archivo
if(fclose(fd_in)<0){
err(1, "No se puede cerrar el archivo %s\n", archivo);
}
}
/* ReadByte - Read a byte of data from the specified id and address
* Return - length of successfully read packet. Return bad number if fails.
*/
int DarwinController::ReadByte(int id, int address, int *byte){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = READ;
txpacket[PARAMETER] = (unsigned char)address;
txpacket[PARAMETER+1] = 1;
txpacket[LENGTH] = 4;
int txlength = txpacket[LENGTH] + 4;
FinishPacket(txpacket); // don't know why i had this commented out.
int result = ReadWrite(txpacket, rxpacket);
if(result == txlength){
*byte = (int)rxpacket[PARAMETER];
}
return result;
}
/******************************************************
* Takes in an instruction and list of params *
* Params needs to be in the form: *
* {ID_1, data_1, ID_2, data_2, ...} *
* There can be any amount of motors and in any order *
* Note that paramlength does not include the ID *
******************************************************/
int DarwinController::SyncWrite(unsigned char* packet, unsigned char instruction, unsigned char* params, unsigned char numparams, unsigned char paramlength){
unsigned char len = numparams + 7; //Last index of array (where checksum goes)
packet[0] = 0xFF; // Heading
packet[1] = 0xFF;
packet[ID] = 0xFE; // Broadcast ID
packet[LENGTH] = len-3; // Length of packet except for headings, ID, Checksum
packet[INSTRUCTION] = 0x83; // Sync Write
packet[5] = instruction; // What is happening at each motor
packet[6] = paramlength; // Number of bytes written to each motor
for(unsigned char i = 0; i < numparams; i++){
packet[7+i] = params[i];
}
packet[len] = CalculateChecksum(packet);
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
return ReadWrite(packet, rxpacket);
//return port.WritePort(packet, len+1); /alt method
}
/* CREAR EL FICHERO .OUT Y SI ESO .OK*/
int
check_ok(char* file_out , char* file_ok, int fd_out){
int success, fd_ok;
if(ExistsFile(file_ok)){
success=Comparate(file_out, file_ok);
if(success){
return 1;
}else{
return 0;
}
}else{
//creamos el archivo .ok
fd_out=open(file_out, O_RDONLY);
fd_ok=CreatFile(file_ok);
//metemos el contenido de .out en .ok
if(fd_out>0 && fd_ok>0){
ReadWrite(fd_out, fd_ok);
}else{
return 0;
}
}
close(fd_ok);
return 1;
}
int main(int argc, char *argv[])
{
int inFileBool = 0;
int outFileBool = 0;
int in_xtcBool = 0;
int out_xtcBool = 0;
int in_trrBool = 0;
int out_trrBool = 0;
char *rfile=NULL, *wfile=NULL;
if(argc != 5)
{
fprintf(stderr,"Usage: %s -i inFile -o outFile\n",argv[0]);
exit(1);
}
else
{
int ii = 1;
while(ii < argc)
{
if(strcmp(argv[ii], "-i") == 0) // if (argv[ii] == "-i")
{
ii++;
inFileBool = 1;
if(strstr(argv[ii], ".xtc") != NULL)
{
in_xtcBool = 1;
in_trrBool = 0;
}
if(strstr(argv[ii], ".trr") != NULL)
{
in_trrBool = 1;
in_xtcBool = 0;
}
rfile = argv[ii];
}
if(strcmp(argv[ii], "-o") == 0)
{
ii++;
outFileBool = 1;
if(strstr(argv[ii], ".xtc") != NULL)
{
out_xtcBool = 1;
out_trrBool = 0;
}
if(strstr(argv[ii], ".trr") != NULL)
{
out_trrBool = 1;
out_xtcBool = 0;
}
wfile = argv[ii];
}
ii++;
}
}
if(!inFileBool || !outFileBool)
{
perror("Usage : ./ReadWrite -i inFile -o outFile");
exit(1);
}
ReadWrite(rfile, wfile, in_xtcBool, out_xtcBool, in_trrBool, out_trrBool);
return 0;
}
static int ReadMemory( addr48_ptr *addr, byte *data, int len )
{
return( ReadWrite( DoReadMem, addr, (char *)data, len ) );
}
static unsigned short WriteMemory( addr48_ptr *addr, byte far *data, unsigned short len )
{
return( ReadWrite( D32DebugWrite, addr, data, len ) );
}
int DarwinController::Set_D_Gain(unsigned char joint_ID, unsigned char D_Value){
unsigned char D_packet[8] = {0, };
MakePacket(D_packet, joint_ID, 1, WRITE, D_GAIN, &D_Value); // Pass in pointer b/c it expects a char*
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(D_packet, rxpacket);
}
/* BulkRead - reads all motor and board information at once into rxpacket
* Return - length of successfully read packet
*/
int DarwinController::BulkRead(unsigned char *rxpacket){
return ReadWrite(BulkReadTxPacket, rxpacket);
}
NTSTATUS FdoPortIoCtl(
IN PC0C_FDOPORT_EXTENSION pDevExt,
IN PIRP pIrp)
{
NTSTATUS status;
PIO_STACK_LOCATION pIrpStack = IoGetCurrentIrpStackLocation(pIrp);
ULONG code = pIrpStack->Parameters.DeviceIoControl.IoControlCode;
KIRQL oldIrql;
PC0C_IO_PORT pIoPortLocal;
pIrp->IoStatus.Information = 0;
pIoPortLocal = pDevExt->pIoPortLocal;
if ((pIoPortLocal->handFlow.ControlHandShake & SERIAL_ERROR_ABORT) &&
pIoPortLocal->errors && code != IOCTL_SERIAL_GET_COMMSTATUS)
{
status = STATUS_CANCELLED;
} else {
status = STATUS_SUCCESS;
switch (code) {
case IOCTL_SERIAL_SET_RTS:
case IOCTL_SERIAL_CLR_RTS:
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
switch (pIoPortLocal->handFlow.FlowReplace & SERIAL_RTS_MASK) {
case SERIAL_RTS_HANDSHAKE:
case SERIAL_TRANSMIT_TOGGLE:
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
status = STATUS_INVALID_PARAMETER;
break;
default: {
LIST_ENTRY queueToComplete;
InitializeListHead(&queueToComplete);
SetModemControl(
pIoPortLocal,
code == IOCTL_SERIAL_SET_RTS ? C0C_MCR_RTS : 0,
C0C_MCR_RTS,
&queueToComplete);
if (pIoPortLocal->pIoPortRemote->tryWrite || pIoPortLocal->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
}
}
break;
case IOCTL_SERIAL_SET_DTR:
case IOCTL_SERIAL_CLR_DTR:
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
switch (pIoPortLocal->handFlow.ControlHandShake & SERIAL_DTR_MASK) {
case SERIAL_DTR_HANDSHAKE:
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
status = STATUS_INVALID_PARAMETER;
break;
default: {
LIST_ENTRY queueToComplete;
InitializeListHead(&queueToComplete);
SetModemControl(
pIoPortLocal,
code == IOCTL_SERIAL_SET_DTR ? C0C_MCR_DTR : 0,
C0C_MCR_DTR,
&queueToComplete);
if (pIoPortLocal->pIoPortRemote->tryWrite || pIoPortLocal->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
}
}
break;
case IOCTL_SERIAL_SET_MODEM_CONTROL: {
LIST_ENTRY queueToComplete;
UCHAR mask;
PUCHAR pSysBuf;
ULONG InputBufferLength;
InputBufferLength = pIrpStack->Parameters.DeviceIoControl.InputBufferLength;
if (InputBufferLength < sizeof(ULONG)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pSysBuf = (PUCHAR)pIrp->AssociatedIrp.SystemBuffer;
if (InputBufferLength >= (sizeof(ULONG) + sizeof(ULONG) + C0CE_SIGNATURE_SIZE) &&
RtlEqualMemory(pSysBuf + sizeof(ULONG) + sizeof(ULONG), C0CE_SIGNATURE, C0CE_SIGNATURE_SIZE))
{
mask = C0C_MCR_MASK & (UCHAR)*((PULONG)pSysBuf + 1);
} else {
mask = C0C_MCR_MASK;
}
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
SetModemControl(pIoPortLocal, (UCHAR)*(PULONG)pSysBuf, mask, &queueToComplete);
if (pIoPortLocal->pIoPortRemote->tryWrite || pIoPortLocal->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
case IOCTL_SERIAL_GET_MODEM_CONTROL:
case IOCTL_SERIAL_GET_DTRRTS: {
ULONG modemControl;
PUCHAR pSysBuf;
ULONG OutputBufferLength;
OutputBufferLength = pIrpStack->Parameters.DeviceIoControl.OutputBufferLength;
if (OutputBufferLength < sizeof(ULONG)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
modemControl = pIoPortLocal->modemControl;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pSysBuf = (PUCHAR)pIrp->AssociatedIrp.SystemBuffer;
if (code == IOCTL_SERIAL_GET_DTRRTS) {
modemControl &= SERIAL_DTR_STATE | SERIAL_RTS_STATE;
pIrp->IoStatus.Information = sizeof(ULONG);
} else {
ULONG InputBufferLength;
InputBufferLength = pIrpStack->Parameters.DeviceIoControl.InputBufferLength;
if (OutputBufferLength >= (sizeof(ULONG) + C0CE_SIGNATURE_SIZE) &&
InputBufferLength >= C0CE_SIGNATURE_SIZE &&
RtlEqualMemory(pSysBuf, C0CE_SIGNATURE, C0CE_SIGNATURE_SIZE))
{
RtlCopyMemory(pSysBuf + sizeof(PULONG), C0CE_SIGNATURE, C0CE_SIGNATURE_SIZE);
if (OutputBufferLength > (sizeof(ULONG) + C0CE_SIGNATURE_SIZE)) {
RtlZeroMemory(pSysBuf + sizeof(ULONG) + C0CE_SIGNATURE_SIZE,
OutputBufferLength - (sizeof(ULONG) + C0CE_SIGNATURE_SIZE));
}
pIrp->IoStatus.Information = OutputBufferLength;
} else {
pIrp->IoStatus.Information = sizeof(ULONG);
}
modemControl &= C0C_MCR_MASK;
}
*(PULONG)pSysBuf = modemControl;
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
}
case IOCTL_SERIAL_SET_XON: {
LIST_ENTRY queueToComplete;
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
SetXonXoffHolding(pIoPortLocal, C0C_XCHAR_ON);
if (pIoPortLocal->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
case IOCTL_SERIAL_SET_XOFF:
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
SetXonXoffHolding(pIoPortLocal, C0C_XCHAR_OFF);
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
case IOCTL_SERIAL_SET_BREAK_ON: {
LIST_ENTRY queueToComplete;
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
SetBreakHolding(pIoPortLocal, TRUE, &queueToComplete);
UpdateTransmitToggle(pIoPortLocal, &queueToComplete);
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
case IOCTL_SERIAL_SET_BREAK_OFF: {
LIST_ENTRY queueToComplete;
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
SetBreakHolding(pIoPortLocal, FALSE, &queueToComplete);
UpdateTransmitToggle(pIoPortLocal, &queueToComplete);
if (pIoPortLocal->tryWrite || pIoPortLocal->pIoPortRemote->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
case IOCTL_SERIAL_GET_MODEMSTATUS:
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(ULONG)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
*(PULONG)pIrp->AssociatedIrp.SystemBuffer = pIoPortLocal->modemStatus;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pIrp->IoStatus.Information = sizeof(ULONG);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_SET_WAIT_MASK:
status = FdoPortSetWaitMask(pIoPortLocal, pIrp, pIrpStack);
break;
case IOCTL_SERIAL_GET_WAIT_MASK:
status = FdoPortGetWaitMask(pIoPortLocal, pIrp, pIrpStack);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_WAIT_ON_MASK:
status = FdoPortWaitOnMask(pIoPortLocal, pIrp, pIrpStack);
#if ENABLE_TRACING
if (status == STATUS_SUCCESS)
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
#endif /* ENABLE_TRACING */
break;
case IOCTL_SERIAL_IMMEDIATE_CHAR:
status = FdoPortImmediateChar(pIoPortLocal, pIrp, pIrpStack);
break;
case IOCTL_SERIAL_XOFF_COUNTER:
status = FdoPortXoffCounter(pIoPortLocal, pIrp, pIrpStack);
break;
case IOCTL_SERIAL_PURGE: {
LIST_ENTRY queueToComplete;
PULONG pSysBuf;
if (pIrpStack->Parameters.DeviceIoControl.InputBufferLength < sizeof(ULONG)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pSysBuf = (PULONG)pIrp->AssociatedIrp.SystemBuffer;
if (*pSysBuf & ~(
SERIAL_PURGE_TXABORT |
SERIAL_PURGE_RXABORT |
SERIAL_PURGE_TXCLEAR |
SERIAL_PURGE_RXCLEAR
)) {
status = STATUS_INVALID_PARAMETER;
break;
}
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
if (*pSysBuf & SERIAL_PURGE_RXABORT)
CancelQueue(&pIoPortLocal->irpQueues[C0C_QUEUE_READ], &queueToComplete);
if (*pSysBuf & SERIAL_PURGE_TXABORT)
CancelQueue(&pIoPortLocal->irpQueues[C0C_QUEUE_WRITE], &queueToComplete);
if (*pSysBuf & SERIAL_PURGE_RXCLEAR) {
PurgeBuffer(&pIoPortLocal->readBuf);
UpdateHandFlow(pIoPortLocal, TRUE, &queueToComplete);
if (pIoPortLocal->tryWrite || pIoPortLocal->pIoPortRemote->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
case IOCTL_SERIAL_GET_COMMSTATUS: {
PSERIAL_STATUS pSysBuf;
PIRP pIrpWrite;
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(SERIAL_STATUS)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pSysBuf = (PSERIAL_STATUS)pIrp->AssociatedIrp.SystemBuffer;
RtlZeroMemory(pSysBuf, sizeof(*pSysBuf));
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
pSysBuf->AmountInInQueue = (ULONG)C0C_BUFFER_BUSY(&pIoPortLocal->readBuf);
pIrpWrite = pIoPortLocal->irpQueues[C0C_QUEUE_WRITE].pCurrent;
if (pIrpWrite) {
PIO_STACK_LOCATION pIrpStackWrite = IoGetCurrentIrpStackLocation(pIrpWrite);
if (pIrpStackWrite->MajorFunction == IRP_MJ_DEVICE_CONTROL &&
pIrpStackWrite->Parameters.DeviceIoControl.IoControlCode == IOCTL_SERIAL_IMMEDIATE_CHAR)
{
pSysBuf->WaitForImmediate = TRUE;
}
}
pSysBuf->AmountInOutQueue = pIoPortLocal->amountInWriteQueue;
pSysBuf->HoldReasons = pIoPortLocal->writeHolding;
if ((pIoPortLocal->handFlow.ControlHandShake & SERIAL_DSR_SENSITIVITY) &&
(pIoPortLocal->modemStatus & C0C_MSB_DSR) == 0)
{
pSysBuf->HoldReasons |= SERIAL_RX_WAITING_FOR_DSR;
}
if (pIoPortLocal->writeHoldingRemote & SERIAL_TX_WAITING_FOR_XON)
pSysBuf->HoldReasons |= SERIAL_TX_WAITING_XOFF_SENT;
pSysBuf->Errors = pIoPortLocal->errors;
pIoPortLocal->errors = 0;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pIrp->IoStatus.Information = sizeof(SERIAL_STATUS);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
}
case IOCTL_SERIAL_SET_HANDFLOW: {
LIST_ENTRY queueToComplete;
PSERIAL_HANDFLOW pSysBuf;
if (pIrpStack->Parameters.DeviceIoControl.InputBufferLength < sizeof(SERIAL_HANDFLOW)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pSysBuf = (PSERIAL_HANDFLOW)pIrp->AssociatedIrp.SystemBuffer;
if (pSysBuf->ControlHandShake & SERIAL_CONTROL_INVALID ||
pSysBuf->FlowReplace & SERIAL_FLOW_INVALID ||
(pSysBuf->ControlHandShake & SERIAL_DTR_MASK) == SERIAL_DTR_MASK ||
pSysBuf->XonLimit < 0 ||
pSysBuf->XoffLimit < 0)
{
status = STATUS_INVALID_PARAMETER;
break;
}
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
status = SetHandFlow(pIoPortLocal, pSysBuf, &queueToComplete);
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
case IOCTL_SERIAL_GET_HANDFLOW:
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(SERIAL_HANDFLOW)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
*(PSERIAL_HANDFLOW)pIrp->AssociatedIrp.SystemBuffer = pIoPortLocal->handFlow;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pIrp->IoStatus.Information = sizeof(SERIAL_HANDFLOW);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_SET_TIMEOUTS:
status = FdoPortSetTimeouts(pIoPortLocal, pIrp, pIrpStack);
break;
case IOCTL_SERIAL_GET_TIMEOUTS:
status = FdoPortGetTimeouts(pIoPortLocal, pIrp, pIrpStack);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_SET_CHARS: {
PSERIAL_CHARS pSysBuf;
if (pIrpStack->Parameters.DeviceIoControl.InputBufferLength < sizeof(SERIAL_CHARS)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pSysBuf = (PSERIAL_CHARS)pIrp->AssociatedIrp.SystemBuffer;
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
if (pIoPortLocal->escapeChar &&
((pIoPortLocal->escapeChar == pSysBuf->XoffChar) ||
(pIoPortLocal->escapeChar == pSysBuf->XonChar)))
{
status = STATUS_INVALID_PARAMETER;
}
if (status == STATUS_SUCCESS)
pIoPortLocal->specialChars = *pSysBuf;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
}
case IOCTL_SERIAL_GET_CHARS:
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(SERIAL_CHARS)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
*(PSERIAL_CHARS)pIrp->AssociatedIrp.SystemBuffer = pIoPortLocal->specialChars;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pIrp->IoStatus.Information = sizeof(SERIAL_CHARS);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_LSRMST_INSERT: {
ULONG Information;
ULONG optsAndBits;
UCHAR escapeChar;
PUCHAR pSysBuf;
ULONG InputBufferLength;
BOOLEAN extended;
InputBufferLength = pIrpStack->Parameters.DeviceIoControl.InputBufferLength;
if (InputBufferLength < sizeof(UCHAR)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
Information = 0;
pSysBuf = (PUCHAR)pIrp->AssociatedIrp.SystemBuffer;
escapeChar = *pSysBuf;
if (InputBufferLength >= (sizeof(UCHAR) + C0CE_SIGNATURE_SIZE + sizeof(ULONG)) &&
RtlEqualMemory(pSysBuf + 1, C0CE_SIGNATURE, C0CE_SIGNATURE_SIZE))
{
extended = TRUE;
optsAndBits = *(ULONG *)(pSysBuf + 1 + C0CE_SIGNATURE_SIZE);
#define C0CE_INSERT_OPTS ( \
C0CE_INSERT_IOCTL_GET| \
C0CE_INSERT_IOCTL_RXCLEAR)
#define C0CE_INSERT_BITS ( \
C0CE_INSERT_ENABLE_LSR| \
C0CE_INSERT_ENABLE_MST| \
C0CE_INSERT_ENABLE_RBR| \
C0CE_INSERT_ENABLE_RLC| \
C0CE_INSERT_ENABLE_LSR_BI)
#define C0CE_INSERT_CAPS (C0CE_INSERT_OPTS|C0CE_INSERT_BITS)
if (optsAndBits == C0CE_INSERT_IOCTL_CAPS) {
optsAndBits = 0;
Information += C0CE_SIGNATURE_SIZE + sizeof(ULONG);
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < Information) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
RtlCopyMemory(pSysBuf, C0CE_SIGNATURE, C0CE_SIGNATURE_SIZE);
*(ULONG *)(pSysBuf + C0CE_SIGNATURE_SIZE) = C0CE_INSERT_CAPS;
} else {
if (optsAndBits & ~C0CE_INSERT_CAPS) {
status = STATUS_INVALID_PARAMETER;
break;
}
if (optsAndBits & C0CE_INSERT_IOCTL_GET) {
if (optsAndBits & (C0CE_INSERT_ENABLE_LSR|C0CE_INSERT_ENABLE_LSR_BI))
Information += sizeof(UCHAR)*2 + sizeof(UCHAR);
if (optsAndBits & C0CE_INSERT_ENABLE_MST)
Information += sizeof(UCHAR)*2 + sizeof(UCHAR);
if (optsAndBits & C0CE_INSERT_ENABLE_RBR)
Information += sizeof(UCHAR)*2 + sizeof(ULONG);
if (optsAndBits & C0CE_INSERT_ENABLE_RLC)
Information += sizeof(UCHAR)*2 + sizeof(UCHAR)*3;
}
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < Information) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
}
} else {
extended = FALSE;
optsAndBits = (C0CE_INSERT_ENABLE_LSR|C0CE_INSERT_ENABLE_MST);
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
if (escapeChar && ((escapeChar == pIoPortLocal->specialChars.XoffChar) ||
(escapeChar == pIoPortLocal->specialChars.XonChar) ||
(pIoPortLocal->handFlow.FlowReplace & SERIAL_ERROR_CHAR)))
{
status = STATUS_INVALID_PARAMETER;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
}
pIoPortLocal->insertMask = optsAndBits & C0CE_INSERT_BITS;
pIoPortLocal->escapeChar = escapeChar;
if (extended) {
LIST_ENTRY queueToComplete;
PC0C_IO_PORT pIoPortRemote;
InitializeListHead(&queueToComplete);
pIoPortRemote = pIoPortLocal->pIoPortRemote;
if (optsAndBits & C0CE_INSERT_IOCTL_GET) {
if (optsAndBits & (C0CE_INSERT_ENABLE_LSR|C0CE_INSERT_ENABLE_LSR_BI)) {
UCHAR lsr = 0;
if (C0C_TX_BUFFER_THR_EMPTY(&pIoPortLocal->txBuf)) {
lsr |= 0x20; /* transmit holding register empty */
if (C0C_TX_BUFFER_EMPTY(&pIoPortLocal->txBuf))
lsr |= 0x40; /* transmit holding register empty and line is idle */
}
if ((optsAndBits & C0CE_INSERT_ENABLE_LSR_BI) != 0 && pIoPortLocal->rcvdBreak)
lsr |= 0x10; /* break interrupt indicator */
*pSysBuf++ = escapeChar;
*pSysBuf++ = SERIAL_LSRMST_LSR_NODATA;
*pSysBuf++ = lsr;
}
if (optsAndBits & C0CE_INSERT_ENABLE_MST) {
*pSysBuf++ = escapeChar;
*pSysBuf++ = SERIAL_LSRMST_MST;
*pSysBuf++ = pIoPortLocal->modemStatus;
}
if (optsAndBits & C0CE_INSERT_ENABLE_RBR) {
*pSysBuf++ = escapeChar;
*pSysBuf++ = C0CE_INSERT_RBR;
*(ULONG *)pSysBuf = pIoPortRemote->baudRate.BaudRate;
pSysBuf += sizeof(ULONG);
}
if (optsAndBits & C0CE_INSERT_ENABLE_RLC) {
*pSysBuf++ = escapeChar;
*pSysBuf++ = C0CE_INSERT_RLC;
*pSysBuf++ = pIoPortRemote->lineControl.WordLength;
*pSysBuf++ = pIoPortRemote->lineControl.Parity;
*pSysBuf++ = pIoPortRemote->lineControl.StopBits;
}
}
pIrp->IoStatus.Information = Information;
if (optsAndBits & C0CE_INSERT_IOCTL_RXCLEAR) {
PurgeBuffer(&pIoPortLocal->readBuf);
UpdateHandFlow(pIoPortLocal, TRUE, &queueToComplete);
if (pIoPortLocal->tryWrite || pIoPortRemote->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortRemote, FALSE,
&queueToComplete);
}
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
}
case IOCTL_SERIAL_SET_LINE_CONTROL: {
PSERIAL_LINE_CONTROL pLineControl;
if (pIrpStack->Parameters.DeviceIoControl.InputBufferLength < sizeof(SERIAL_LINE_CONTROL)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pLineControl = (PSERIAL_LINE_CONTROL)pIrp->AssociatedIrp.SystemBuffer;
switch (pLineControl->WordLength) {
case 5:
case 6:
case 7:
case 8:
break;
default:
status = STATUS_INVALID_PARAMETER;
}
switch (pLineControl->Parity) {
case NO_PARITY:
case ODD_PARITY:
case EVEN_PARITY:
case MARK_PARITY:
case SPACE_PARITY:
break;
default:
status = STATUS_INVALID_PARAMETER;
}
switch (pLineControl->StopBits) {
case STOP_BIT_1:
case STOP_BITS_1_5:
case STOP_BITS_2:
break;
default:
status = STATUS_INVALID_PARAMETER;
}
if (status == STATUS_INVALID_PARAMETER)
break;
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
if (pIoPortLocal->lineControl.StopBits != pLineControl->StopBits ||
pIoPortLocal->lineControl.Parity != pLineControl->Parity ||
pIoPortLocal->lineControl.WordLength != pLineControl->WordLength)
{
PC0C_IO_PORT pIoPortRemote;
pIoPortLocal->lineControl = *pLineControl;
SetWriteDelay(pIoPortLocal);
pIoPortRemote = pIoPortLocal->pIoPortRemote;
if (pIoPortRemote->escapeChar && (pIoPortRemote->insertMask & C0CE_INSERT_ENABLE_RLC)) {
LIST_ENTRY queueToComplete;
InitializeListHead(&queueToComplete);
InsertRemoteLc(pIoPortRemote, &queueToComplete);
if (pIoPortLocal->pIoPortRemote->tryWrite || pIoPortLocal->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
}
case IOCTL_SERIAL_GET_LINE_CONTROL:
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(SERIAL_LINE_CONTROL)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
*(PSERIAL_LINE_CONTROL)pIrp->AssociatedIrp.SystemBuffer = pIoPortLocal->lineControl;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pIrp->IoStatus.Information = sizeof(SERIAL_LINE_CONTROL);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_SET_BAUD_RATE: {
PSERIAL_BAUD_RATE pBaudRate;
if (pIrpStack->Parameters.DeviceIoControl.InputBufferLength < sizeof(SERIAL_BAUD_RATE)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pBaudRate = (PSERIAL_BAUD_RATE)pIrp->AssociatedIrp.SystemBuffer;
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
if (pIoPortLocal->baudRate.BaudRate != pBaudRate->BaudRate) {
PC0C_IO_PORT pIoPortRemote;
pIoPortLocal->baudRate = *pBaudRate;
SetWriteDelay(pIoPortLocal);
pIoPortRemote = pIoPortLocal->pIoPortRemote;
if (pIoPortRemote->escapeChar && (pIoPortRemote->insertMask & C0CE_INSERT_ENABLE_RBR)) {
LIST_ENTRY queueToComplete;
InitializeListHead(&queueToComplete);
InsertRemoteBr(pIoPortRemote, &queueToComplete);
if (pIoPortLocal->pIoPortRemote->tryWrite || pIoPortLocal->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
}
case IOCTL_SERIAL_GET_BAUD_RATE:
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(SERIAL_BAUD_RATE)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
*(PSERIAL_BAUD_RATE)pIrp->AssociatedIrp.SystemBuffer = pIoPortLocal->baudRate;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pIrp->IoStatus.Information = sizeof(SERIAL_BAUD_RATE);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_GET_PROPERTIES: {
ULONG size;
status = GetCommProp(pDevExt,
pIrp->AssociatedIrp.SystemBuffer,
pIrpStack->Parameters.DeviceIoControl.OutputBufferLength,
&size);
if (status == STATUS_SUCCESS) {
pIrp->IoStatus.Information = size;
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
}
break;
}
case IOCTL_SERIAL_CONFIG_SIZE:
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(ULONG)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pIrp->IoStatus.Information = sizeof(ULONG);
*(PULONG)pIrp->AssociatedIrp.SystemBuffer = 0;
break;
case IOCTL_SERIAL_SET_QUEUE_SIZE: {
PSERIAL_QUEUE_SIZE pSysBuf = (PSERIAL_QUEUE_SIZE)pIrp->AssociatedIrp.SystemBuffer;
LIST_ENTRY queueToComplete;
PC0C_BUFFER pReadBuf;
PUCHAR pBase;
if (pIrpStack->Parameters.DeviceIoControl.InputBufferLength < sizeof(SERIAL_QUEUE_SIZE)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
pReadBuf = &pIoPortLocal->readBuf;
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
if (pSysBuf->InSize <= C0C_BUFFER_SIZE(pReadBuf)) {
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
try {
pBase = C0C_ALLOCATE_POOL_WITH_QUOTA(NonPagedPool, pSysBuf->InSize);
}
except (EXCEPTION_EXECUTE_HANDLER) {
pBase = NULL;
status = GetExceptionCode();
}
if (!pBase)
break;
InitializeListHead(&queueToComplete);
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
if (SetNewBufferBase(pReadBuf, pBase, pSysBuf->InSize)) {
pIoPortLocal->handFlow.XoffLimit = pSysBuf->InSize >> 3;
pIoPortLocal->handFlow.XonLimit = pSysBuf->InSize >> 1;
SetLimit(pIoPortLocal);
UpdateHandFlow(pIoPortLocal, TRUE, &queueToComplete);
if (pIoPortLocal->tryWrite || pIoPortLocal->pIoPortRemote->tryWrite) {
ReadWrite(
pIoPortLocal, FALSE,
pIoPortLocal->pIoPortRemote, FALSE,
&queueToComplete);
}
}
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
FdoPortCompleteQueue(&queueToComplete);
break;
}
case IOCTL_SERIAL_GET_STATS:
if (pIrpStack->Parameters.DeviceIoControl.OutputBufferLength < sizeof(SERIALPERF_STATS)) {
status = STATUS_BUFFER_TOO_SMALL;
break;
}
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
*(PSERIALPERF_STATS)pIrp->AssociatedIrp.SystemBuffer = pIoPortLocal->perfStats;
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
pIrp->IoStatus.Information = sizeof(SERIALPERF_STATS);
TraceIrp("FdoPortIoCtl", pIrp, &status, TRACE_FLAG_RESULTS);
break;
case IOCTL_SERIAL_CLEAR_STATS:
KeAcquireSpinLock(pIoPortLocal->pIoLock, &oldIrql);
RtlZeroMemory(&pIoPortLocal->perfStats, sizeof(pIoPortLocal->perfStats));
KeReleaseSpinLock(pIoPortLocal->pIoLock, oldIrql);
break;
default:
status = STATUS_INVALID_PARAMETER;
}
}
// is_enabled is a 0 for no torque or 1 for torque
int DarwinController::Set_Torque_Enable(unsigned char joint_ID, unsigned char is_enabled){
unsigned char Torque_packet[8] = {0, };
MakePacket(Torque_packet, joint_ID, 1, WRITE, TORQUE_ENABLE, &is_enabled);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(Torque_packet, rxpacket);
}
static int WriteMemory( addr48_ptr *addr, byte *data, int len )
{
if( addr->segment == Regs.CS ) addr->segment = Regs.DS; // hack, ack
return( ReadWrite( DoWriteMem, addr, (char *)data, len ) );
}
NTSTATUS SetHandFlow(
PC0C_IO_PORT pIoPort,
PSERIAL_HANDFLOW pHandFlow,
PLIST_ENTRY pQueueToComplete)
{
UCHAR bits, mask;
PC0C_BUFFER pReadBuf;
PSERIAL_HANDFLOW pNewHandFlow;
BOOLEAN setModemStatusHolding;
pReadBuf = &pIoPort->readBuf;
if (pHandFlow) {
if ((pIoPort->escapeChar && (pHandFlow->FlowReplace & SERIAL_ERROR_CHAR)) ||
((SIZE_T)pHandFlow->XonLimit > C0C_BUFFER_SIZE(pReadBuf)) ||
((SIZE_T)pHandFlow->XoffLimit > C0C_BUFFER_SIZE(pReadBuf)))
{
return STATUS_INVALID_PARAMETER;
}
pNewHandFlow = pHandFlow;
} else {
pNewHandFlow = &pIoPort->handFlow;
}
// Set local side
if (!pHandFlow)
pIoPort->writeHolding &= ~SERIAL_TX_WAITING_FOR_XON;
if (pHandFlow &&
((pIoPort->handFlow.FlowReplace & SERIAL_AUTO_TRANSMIT) != 0) &&
((pHandFlow->FlowReplace & SERIAL_AUTO_TRANSMIT) == 0))
{
SetXonXoffHolding(pIoPort, C0C_XCHAR_ON);
}
if (!pHandFlow ||
(pIoPort->handFlow.ControlHandShake & SERIAL_OUT_HANDSHAKEMASK) !=
(pHandFlow->ControlHandShake & SERIAL_OUT_HANDSHAKEMASK))
{
setModemStatusHolding = TRUE;
} else {
setModemStatusHolding = FALSE;
}
// Set remote side
bits = mask = 0;
if (!pHandFlow ||
(pIoPort->handFlow.FlowReplace & SERIAL_RTS_MASK) !=
(pHandFlow->FlowReplace & SERIAL_RTS_MASK))
{
switch (pNewHandFlow->FlowReplace & SERIAL_RTS_MASK) {
case 0:
pIoPort->writeHoldingRemote &= ~SERIAL_TX_WAITING_FOR_CTS;
mask |= C0C_MCR_RTS;
break;
case SERIAL_RTS_CONTROL:
pIoPort->writeHoldingRemote &= ~SERIAL_TX_WAITING_FOR_CTS;
bits |= C0C_MCR_RTS;
mask |= C0C_MCR_RTS;
break;
case SERIAL_RTS_HANDSHAKE:
if (C0C_BUFFER_BUSY(pReadBuf) > (C0C_BUFFER_SIZE(pReadBuf) - pNewHandFlow->XoffLimit)) {
pIoPort->writeHoldingRemote |= SERIAL_TX_WAITING_FOR_CTS;
mask |= C0C_MCR_RTS;
}
else
if (pIoPort->writeHoldingRemote & SERIAL_TX_WAITING_FOR_CTS) {
if (C0C_BUFFER_BUSY(pReadBuf) <= (SIZE_T)pNewHandFlow->XonLimit) {
pIoPort->writeHoldingRemote &= ~SERIAL_TX_WAITING_FOR_CTS;
bits |= C0C_MCR_RTS;
mask |= C0C_MCR_RTS;
}
}
else {
bits |= C0C_MCR_RTS;
mask |= C0C_MCR_RTS;
}
}
}
if (!pHandFlow ||
(pIoPort->handFlow.ControlHandShake & SERIAL_DTR_MASK) !=
(pHandFlow->ControlHandShake & SERIAL_DTR_MASK))
{
switch (pNewHandFlow->ControlHandShake & SERIAL_DTR_MASK) {
case 0:
pIoPort->writeHoldingRemote &= ~SERIAL_TX_WAITING_FOR_DSR;
mask |= C0C_MCR_DTR;
break;
case SERIAL_DTR_CONTROL:
pIoPort->writeHoldingRemote &= ~SERIAL_TX_WAITING_FOR_DSR;
bits |= C0C_MCR_DTR;
mask |= C0C_MCR_DTR;
break;
case SERIAL_DTR_HANDSHAKE:
if (C0C_BUFFER_BUSY(pReadBuf) > (C0C_BUFFER_SIZE(pReadBuf) - pNewHandFlow->XoffLimit)) {
pIoPort->writeHoldingRemote |= SERIAL_TX_WAITING_FOR_DSR;
mask |= C0C_MCR_DTR;
}
else
if (pIoPort->writeHoldingRemote & SERIAL_TX_WAITING_FOR_DSR) {
if (C0C_BUFFER_BUSY(pReadBuf) <= (SIZE_T)pNewHandFlow->XonLimit) {
pIoPort->writeHoldingRemote &= ~SERIAL_TX_WAITING_FOR_DSR;
bits |= C0C_MCR_DTR;
mask |= C0C_MCR_DTR;
}
}
else {
bits |= C0C_MCR_DTR;
mask |= C0C_MCR_DTR;
}
}
}
if (!pHandFlow ||
(pIoPort->handFlow.FlowReplace & SERIAL_AUTO_RECEIVE) !=
(pHandFlow->FlowReplace & SERIAL_AUTO_RECEIVE))
{
if (pNewHandFlow->FlowReplace & SERIAL_AUTO_RECEIVE) {
if (C0C_BUFFER_BUSY(pReadBuf) > (C0C_BUFFER_SIZE(pReadBuf) - pNewHandFlow->XoffLimit)) {
pIoPort->writeHoldingRemote |= SERIAL_TX_WAITING_FOR_XON;
if ((pNewHandFlow->FlowReplace & SERIAL_XOFF_CONTINUE) == 0)
pIoPort->writeHolding |= SERIAL_TX_WAITING_FOR_XON;
pIoPort->sendXonXoff = C0C_XCHAR_OFF;
pIoPort->tryWrite = TRUE;
}
}
else
if (pIoPort->writeHoldingRemote & SERIAL_TX_WAITING_FOR_XON) {
pIoPort->writeHoldingRemote &= ~SERIAL_TX_WAITING_FOR_XON;
pIoPort->writeHolding &= ~SERIAL_TX_WAITING_FOR_XON;
if (pIoPort->sendXonXoff != C0C_XCHAR_OFF) {
// XOFF was sent so send XON
pIoPort->sendXonXoff = C0C_XCHAR_ON;
pIoPort->tryWrite = TRUE;
} else {
// XOFF still was not sent so cancel it
pIoPort->sendXonXoff = 0;
}
}
}
if (pHandFlow)
pIoPort->handFlow = *pHandFlow;
if (setModemStatusHolding)
SetModemStatusHolding(pIoPort);
if (mask)
SetModemControl(pIoPort, bits, mask, pQueueToComplete);
UpdateTransmitToggle(pIoPort, pQueueToComplete);
SetLimit(pIoPort->pIoPortRemote);
SetLimit(pIoPort);
if (pIoPort->pIoPortRemote->tryWrite) {
ReadWrite(
pIoPort, FALSE,
pIoPort->pIoPortRemote, FALSE,
pQueueToComplete);
}
if (pIoPort->tryWrite) {
ReadWrite(
pIoPort->pIoPortRemote, FALSE,
pIoPort, FALSE,
pQueueToComplete);
}
return STATUS_SUCCESS;
}
