// Make a copy of message for task handler.
// If pktLen!=0, sets idx and copy pktLen to that.
// Returns new index if successful.
int CopyMessage(int idx, byte pktLen)
{
int ioIndex = Manager.IoBuffersFreeHeap.Allocate();
if (ioIndex != -1)
{
PacketBuffer a = Manager.IoBuffers[idx];
PacketBuffer b = Manager.IoBuffers[ioIndex];
if (pktLen == 0)
{
pktLen = (byte)(ProtocolConstants.GENERAL_OVERHEADS_SIZE + a.dataLength());
}
else
{
a.pktLen = pktLen;
}
b.meta(a);
Array.Copy(a.buffer, 0, b.buffer, 0, pktLen);
}
return(ioIndex);
}
// Create a command general message.
public int CreateGeneralMessage(bool verified, byte cmd,
ushort receiver, ushort sender, byte flagsRS, byte senderId,
byte dataLen, byte[] pData, byte bufferIndex)
{
if (dataLen > (Manager.MaxPacketSize - ProtocolConstants.GENERAL_OVERHEADS_SIZE))
{
return(-1);
}
int idx = Manager.IoBuffersFreeHeap.Allocate();
if (idx != -1)
{
PacketBuffer b = Manager.IoBuffers[idx];
byte pid = verified ? ProtocolConstants.PID_GENERAL_V : ProtocolConstants.PID_GENERAL;
b.flagsPid = pid;
b.dNetIf = ProtocolConstants.NETIF_UNSET;
b.iNetIf = ProtocolConstants.NETIF_UNSET;
b.pktLen = (byte)(ProtocolConstants.GENERAL_OVERHEADS_SIZE + dataLen);
b.sync(ProtocolConstants.BYTE_SYNC);
b.negPidPid(Primitives.NibbleToPid(pid));
b.destination(LinkMonitor.Destination); // TODO: set destination.
b.arg(0);
b.receiver(receiver);
b.sender(sender);
b.senderId(senderId);
b.flagsRS(flagsRS);
b.command(cmd);
b.dataLength(dataLen);
byte dataIndex = ProtocolConstants.GENERAL_DATA_ARRAY_OFFSET;
byte[] p = b.buffer;
for (byte k = 0; k < dataLen; k++)
{
p[dataIndex++] = pData[bufferIndex + k];
}
b.UpdateCheckSum(b.pktLen);
}
return(idx);
}
// Helper routine for accessing messages.
// ioIndex must point to a valid message, i.e. meta is set.
// Returns 0 on success.
// Returns error code on parse failure.
public byte StartMessageTransaction(int ioIndex)
{
MsgBuffer = Manager.IoBuffers[ioIndex];
IoIndex = ioIndex;
ChangeDir = false;
Finish = FinishAction.Free; // Free messages by default: prevents unhandled recursions.
Dispatch = Router.RouterAction.PostToNetIf;
TaskCmd = 0;
ReturnCmd = 0;
ONetIfIndex = MsgBuffer.iNetIf;
switch (MsgBuffer.flagsPid & ProtocolConstants.META_FLAGS_PID)
{
case ProtocolConstants.PID_GENERAL_V:
MsgBuffer.flagsPid &= ProtocolConstants.META_FLAGS_PID; // Clear out any resend value.
break;
case ProtocolConstants.PID_GENERAL:
break;
default:
return(100);
}
return(0);
}
// Submit a handshake command to link WriteQ.
// For example, Resend Vno command, ping, reply, etc.
public int SendLinkHandshakePacket(byte pid, ushort destination, byte arg)
{
int idx = Manager.IoBuffersFreeHeap.Allocate();
if (idx != -1)
{
PacketBuffer b = Manager.IoBuffers[idx];
b.flagsPid = pid;
b.dNetIf = ProtocolConstants.NETIF_UNSET;
b.iNetIf = ProtocolConstants.NETIF_UNSET;
b.pktLen = ProtocolConstants.HANDSHAKE_PACKET_SIZE;
b.sync(ProtocolConstants.BYTE_SYNC);
b.negPidPid(Primitives.NibbleToPid(pid));
b.destination(destination);
b.arg(arg);
b.UpdateCheckSum(b.pktLen);
if (IoNetIf.LinkWriteQueue.PushFirst(idx) == -1)
{
Manager.IoBuffersFreeHeap.Release(idx);
return(-1);
}
}
return(idx);
}
public static string DumpHandshake(PacketBuffer b)
{
return("{" + String.Format("pid={0:x2},arg={1:x2}", b.negPidPid() & ProtocolConstants.PID_MASK, b.arg()) + "}\n");
}
// On entry: ParseState is current protocol parser state.
public void ProtocolParser(byte[] buffer)
{
int bufferIndex = 0, numRead = buffer.Length;
while (bufferIndex < numRead)
{
byte b = buffer[bufferIndex++];
switch (ParseEscState)
{
case PARSE_ESC_SECOND:
ParseEscState = PARSE_ESC_START;
switch (b)
{
case ProtocolConstants.BYTE_ESC_SYNC:
if (ParseState == PARSE_SYNC)
{
continue;
}
b = ProtocolConstants.BYTE_SYNC;
break;
case ProtocolConstants.BYTE_ESC_ESC:
b = ProtocolConstants.BYTE_ESC;
break;
default:
break;
}
break;
case PARSE_ESC_START:
if (b == ProtocolConstants.BYTE_ESC)
{
ParseEscState = PARSE_ESC_SECOND;
continue;
}
else if (b == ProtocolConstants.BYTE_SYNC)
{
ParseState = PARSE_SYNC;
}
break;
default:
break;
}
switch (ParseState)
{
case PARSE_SYNC:
if (b == ProtocolConstants.BYTE_SYNC)
{
ParseState = PARSE_PID;
}
break;
case PARSE_PID:
Pid = (byte)(b & ProtocolConstants.PID_MASK);
switch (Pid)
{
case ProtocolConstants.PID_PING:
case ProtocolConstants.PID_REPLY:
case ProtocolConstants.PID_RESEND:
case ProtocolConstants.PID_CANCEL:
InputBuffer = HandshakePacket.buffer;
PktLen = ProtocolConstants.HANDSHAKE_PACKET_SIZE;
break;
case ProtocolConstants.PID_GENERAL:
case ProtocolConstants.PID_GENERAL_V:
if (IoIndex == -1) // Setup a buffer.
{
IoIndex = Manager.IoBuffersFreeHeap.Allocate();
if (IoIndex == -1)
{
ParseState = PARSE_SYNC;
continue;
}
CurrentPacket = Manager.IoBuffers[IoIndex];
}
InputBuffer = CurrentPacket.buffer;
PktLen = ProtocolConstants.GENERAL_HEADER_SIZE;
break;
default:
ParseState = PARSE_SYNC;
continue;
}
InputBuffer[ProtocolConstants.PACKET_SYNC_OFFSET] = ProtocolConstants.BYTE_SYNC;
InputBuffer[ProtocolConstants.PACKET_PID_OFFSET] = b;
DataIndex = ProtocolConstants.CHECK_START_OFFSET; // Skip over sync & pid.
ChkSum = 0;
ParseState = PARSE_HEADER;
break;
case PARSE_HEADER:
ChkSum += b;
InputBuffer[DataIndex++] = b;
if (DataIndex < PktLen)
{
break;
}
if (Pid > ProtocolConstants.PID_HANDSHAKE_MAX)
{
byte len = InputBuffer[ProtocolConstants.GENERAL_DATA_LENGTH_OFFSET];
PktLen = (byte)(len + ProtocolConstants.GENERAL_OVERHEADS_SIZE);
if (len <= (Manager.MaxPacketSize - ProtocolConstants.GENERAL_OVERHEADS_SIZE))
{
ParseState = PARSE_DATA;
break;
}
if (Settings.DebugLevel > 2)
{
Log.w(TAG, "Bad message length:" + len + ".");
}
}
else
{
if ((ChkSum % 256) == 0)
{
IoNetIf.Monitor.PerformLinkHandshake(HandshakePacket);
if (Settings.DebugLevel > 6)
{
Log.d(TAG, Diagnostics.DumpHandshake(HandshakePacket));
}
}
else
{
if (Settings.DebugLevel > 2)
{
Log.w(TAG, "Bad header chksum.");
}
}
}
ParseState = PARSE_SYNC;
break;
case PARSE_DATA:
ChkSum += b;
InputBuffer[DataIndex++] = b;
if (DataIndex < PktLen)
{
break;
}
if ((ChkSum % 256) != 0) // Xfer error, ditch packet.
{
if (Settings.DebugLevel > 2)
{
Log.w(TAG, "Bad message chksum.");
}
ParseState = PARSE_SYNC;
break;
}
// By here there is a valid packet.
// Fill in some missing details:
CurrentPacket.flagsPid = Pid;
CurrentPacket.pktLen = PktLen;
CurrentPacket.iNetIf = IoNetIf.NetIfIndex;
CurrentPacket.dNetIf = ProtocolConstants.NETIF_UNSET;
if (Settings.DebugLevel > 6)
{
Log.d(TAG, Diagnostics.DumpMessage(CurrentPacket));
}
IoNetIf.Monitor.DispatchLinkPacket(this);
ParseState = PARSE_SYNC;
break;
default:
ParseState = PARSE_SYNC;
break;
}
}
}
// Stream message from WriteQ and send to output device or message handler.
// Returns 0 on success, 1 if caller should retry write.
public byte LinkWrite()
{
if (DoesIO)
{
if (IoNetIfDevice.WriteBufferEmpty(DeviceContextIndex))
{
int idx, n; byte[] q;
if (NumOverflowed > 0)
{
q = OutputMapBuffer;
n = NumOverflowed;
NumOverflowed = 0;
IoNetIfDevice.Write(q, Manager.MaxPacketSize, n);
}
else if ((idx = LinkWriteQueue.Peek()) != -1)
{
PacketBuffer b = Manager.IoBuffers[idx];
byte[] r;
byte verifyAction = Monitor.PreLinkWriteFilter(b);
n = b.pktLen;
q = b.buffer;
if (MappingEnabled && ((r = OutputMapBuffer) != null))
{
byte c;
int j, k = 0;
if (n-- > 0) // Step over Sync byte.
{
r[k] = q[k];
++k;
}
j = k;
while (n-- > 0)
{
c = q[k++];
if ((c == ProtocolConstants.BYTE_SYNC) || (c == ProtocolConstants.BYTE_ESC))
{
r[j++] = ProtocolConstants.BYTE_ESC;
c = (c == ProtocolConstants.BYTE_ESC ? ProtocolConstants.BYTE_ESC_ESC : ProtocolConstants.BYTE_ESC_SYNC);
}
r[j++] = c;
}
n = j;
q = OutputMapBuffer;
}
if (n > Manager.MaxPacketSize) // If message bigger than packet, chop in two.
{
NumOverflowed = (byte)n;
n = Manager.MaxPacketSize;
}
if (!IoNetIfDevice.Write(q, 0, n))
{
NumOverflowed = 0;
// TODO: Count failures. Take action: mark item as unsent, flush, place on end of queue?
++NumIOAttempts;
if (NumIOAttempts >= 3)
{
Thread.Sleep(200);
}
return(1);
}
if (LinkWriteQueue.PopIfEqual(idx) == idx) // Check if detach/flush occurred.
{
Monitor.PostLinkWriteFilter(idx, verifyAction);
NumIOAttempts = 0;
}
}
}
else
{
return(1);
}
}
else
{
int n;
byte r;
// Call messageHandler on WriteQ.
while ((n = LinkWriteQueue.Pop()) != -1)
{
r = MessageCommandHandler(n);
if (r != 0)
{
Manager.IoBuffersFreeHeap.Release(n);
}
}
}
return(0);
}
// Perform a handshake request received from link.
// Consults original message for r,s details, ports=0, flags=0.
// TODO: check what happens when a master is pinged etc. Ok?
public void PerformLinkHandshake(PacketBuffer hp)
{
int idx;
byte arg = hp.arg();
// TODO: Check packet is for us first.
switch (hp.negPidPid() & ProtocolConstants.PID_MASK)
{
case ProtocolConstants.PID_PING: // We received a ping. Now send a reply.
if (arg >= ProtocolConstants.PID_ARG_BAUD_MIN)
{
switch (arg)
{
case ProtocolConstants.PID_ARG_RESET:
IoNetIf.ResetLinkDriver();
break;
case ProtocolConstants.PID_ARG_SLAVE:
break;
case ProtocolConstants.PID_ARG_MULTI:
break;
case ProtocolConstants.PID_ARG_MASTER:
break;
default: // BAUD: Request to change baud rate.
IoNetIf.IoNetIfDevice.PerformBaudAction(IoNetIf.DeviceContextIndex,
(byte)(arg - ProtocolConstants.PID_ARG_BAUD_MIN),
ProtocolConstants.BAUD_ACTION_SAVE);
break;
}
}
else
{
// Set linkLastInputVno=(VNO_SIZE-1) & linkLastOutputVno=0 ?
CheckOrderMissingQ(arg, false);
IoNetIf.duplexPingReply |= LinkMonitor.DUX_PING_RECEIVED;
}
// Send a reply with Arg = num messages waiting.
// TODO: unless Q contains Msg destined to sender: use "more" msg code.
int num = IoNetIf.LinkWriteQueue.Size(); // Arg = num messages waiting.
SendLinkHandshakePacket(ProtocolConstants.PID_REPLY,
LinkMonitor.Destination, // hp.destination(), // TODO: This should be the linkAdrs.
(byte)((num > 255) ? 255 : num));
break;
case ProtocolConstants.PID_REPLY: //TODO: Now stop pinging.
// arg=number of waiting messages.
// If arg>0 read some messages.
IoNetIf.duplexNumWaiting = arg;
IoNetIf.duplexPingReply |= LinkMonitor.DUX_REPLY_RECEIVED;
break;
case ProtocolConstants.PID_RESEND: // Resend a message.
idx = FindVnoInVerifyQueue(IoNetIf.LinkVerifyQueue, arg);
if (idx == -1)
{
SendLinkHandshakePacket(ProtocolConstants.PID_CANCEL, hp.destination(), arg);
}
else if (IoNetIf.LinkWriteQueue.FindFirstOccurrenceIndex(idx) == -1)
{
PacketBuffer b = Manager.IoBuffers[idx];
b.flagsPid |= ProtocolConstants.META_FLAGS_RESEND;
if (IoNetIf.LinkWriteQueue.PushFirst(idx) == -1)
{
Manager.IoBuffersFreeHeap.Release(idx);
}
}
break;
case ProtocolConstants.PID_CANCEL: // Remove message from missingQ.
IoNetIf.LinkMissingQueue.RemoveFirstOccurrence(arg);
break;
default: break;
}
// TODO: IoNetIf.activityTimeout=Primitives.GetBabelMilliTicker() + LinkMonitor.CONX_ACTIVITY_TIMEOUT;
IoNetIf.baudTimeout = Primitives.GetBabelMilliTicker() + LinkMonitor.BAUD_RATE_TIMEOUT; // Reset timer after input from specific sender.
}
// Called just before message is written to device.
// Determine if packet needs modification.
// Updates DUX & BAUD flags.
// Returns verification action if needed.
// TODO: set destination.
public byte PreLinkWriteFilter(PacketBuffer b)
{
int sumDiff = 0;
byte verifyAction = ProtocolConstants.VERIFY_ACTION_NONE;
byte c = (byte)(b.flagsPid & ProtocolConstants.META_FLAGS_PID);
if (c == ProtocolConstants.PID_GENERAL_V)
{
// Sequence numbers are used to detect bad xfers.
// Resends must be sent with the same Vno rather than a new Vno.
if ((b.flagsPid & ProtocolConstants.META_FLAGS_RESEND) != 0)
{
verifyAction = ProtocolConstants.VERIFY_ACTION_RESEND; // This stops vnoLastOutput being incremented below.
}
else
{
verifyAction = ProtocolConstants.VERIFY_ACTION_NEW;
sumDiff = (int)b.arg() - (int)IoNetIf.vnoLastOutput;
b.arg(IoNetIf.vnoLastOutput);
}
}
else if (c == ProtocolConstants.PID_PING)
{
byte arg = b.arg();
if (arg == 0) // It's a basic ping, so set arg to vno.
{
sumDiff = -(int)IoNetIf.vnoLastOutput;
b.arg(IoNetIf.vnoLastOutput);
}
else if (arg >= ProtocolConstants.PID_ARG_BAUD_MIN)
{
switch (arg)
{
case ProtocolConstants.PID_ARG_RESET:
break;
case ProtocolConstants.PID_ARG_SLAVE:
break;
case ProtocolConstants.PID_ARG_MULTI:
break;
case ProtocolConstants.PID_ARG_MASTER:
break;
default: // BAUD: Flag baud ping sent, set UART_*_SetBaudRate = netIfIndex.
// This is a broadcast to all on link.
IoNetIf.IoNetIfDevice.PerformBaudAction(IoNetIf.DeviceContextIndex,
(byte)(arg - ProtocolConstants.PID_ARG_BAUD_MIN),
ProtocolConstants.BAUD_ACTION_SIGNAL);
IoNetIf.baudTimeout = Primitives.GetBabelMilliTicker() + LinkMonitor.BAUD_RATE_TIMEOUT; // Mark Baud broadcast for all.
break;
}
}
// DUX: need to block further writes.
if ((IoNetIf.duplexKind & (byte)LINK_DUPLEX_KIND.LINK_DUPLEX_KIND_HALF) != 0)
{
IoNetIf.duplexPingReply |= LinkMonitor.DUX_PING_SENT;
IoNetIf.duplexNumWaiting = (byte)0xffu;
IoNetIf.duplexMode = (byte)DUX_MODE.DUX_RX;
}
}
else if (c == ProtocolConstants.PID_REPLY)
{
IoNetIf.duplexPingReply |= LinkMonitor.DUX_REPLY_SENT;// DUX: flag reply sent.
}
// Update checksum: assume valid on entry & work out bit change.
if (sumDiff != 0)
{
// newChkSum = oldChkSum + (oldSumBits - newSumBits).
byte oldChkSum = b.buffer[b.pktLen - 1];
b.buffer[b.pktLen - 1] = (byte)(((int)oldChkSum) + sumDiff);
}
return(verifyAction);
}
// Process Port commands.
// Returns zero on success.
public override byte CommandHandler(MessageTransaction mtx)
{
PacketBuffer g = mtx.MsgBuffer;
Boolean isReply = ((g.flagsRS() & ProtocolConstants.MESSAGE_FLAGS_IS_REPLY) != 0);
if (((!isReply) && (g.command() < ProtocolConstants.MEDIATOR_CONTROL_CMD_BASE)) ||
(isReply && (g.senderId() == ProtocolConstants.IDENT_MEDIATOR)))
{
byte dataIndex = ProtocolConstants.GENERAL_DATA_ARRAY_OFFSET;
byte[] tmp;
switch (g.command())
{
// Standard device commands.
case ProtocolConstants.MEDIATOR_DEVICE_RESET: // Reset the module.
Manager.AppBabelDeviceReset();
break;
case ProtocolConstants.MEDIATOR_DEVICE_STATUS: // Get device status.
if (isReply)
{
return(1);
}
g.dataLength(Diagnostics.GetDeviceStatus(Manager, g.buffer));
mtx.ChangeDir = true;
mtx.Finish = MessageTransaction.FinishAction.Normal;
break;
case ProtocolConstants.MEDIATOR_DEVICE_TICKER: // Get device ticker count.
if (isReply)
{
return(1); // We've been sent a ticker count.
}
// Sender wants to know ticker count.
tmp = Primitives.UIntToByteArray(Primitives.GetBabelMilliTicker());
return(mtx.StoreMessageValue(tmp, (byte)(tmp.Length)));
case ProtocolConstants.MEDIATOR_DEVICE_ERASE: // TODO: erase eeprom and restart.
return(2);
case ProtocolConstants.MEDIATOR_DEVICE_READVAR:
return((MediatorReadCommand(mtx) == 0) ? (byte)1 : (byte)0);
case ProtocolConstants.MEDIATOR_DEVICE_WRITEVAR:
return((MediatorWriteCommand(mtx) == 0) ? (byte)1 : (byte)0);
case ProtocolConstants.MEDIATOR_DEVICE_ISOVAR:
return((MediatorIsoCommand(mtx) == 0) ? (byte)1 : (byte)0);
case ProtocolConstants.MEDIATOR_DEVICE_ISOMONVAR:
return((MediatorIsoCommand(mtx) == 0) ? (byte)1 : (byte)0);
case ProtocolConstants.MEDIATOR_DEVICE_ISOMSG:
return((MediatorIsoCommand(mtx) == 0) ? (byte)1 : (byte)0);
case ProtocolConstants.MEDIATOR_DEVICE_LOG:
if (!isReply)
{
if (Settings.LogOn)
{
uint ticks = (uint)Primitives.GetArrayValueU(g.buffer, dataIndex, 4);
string s;
byte logType = g.buffer[dataIndex + 4];
if (Settings.LogBinary || logType == 'B')
{
int start = dataIndex + 5;
int last = dataIndex + g.dataLength();
s = "(";
for (int k = start; k < last; k++)
{
s = s + g.buffer[k].ToString("X2") + " ";
}
s += ")";
}
else
{
s = Primitives.GetArrayStringValue(g.buffer, dataIndex + 5, g.dataLength() - 5);
}
Log.d("MediatorNetIf:" + Exchange.Id, ":" + ticks + ":" + s);
}
}
return(1);
// Connection specific commands.
case ProtocolConstants.MEDIATOR_DEVICE_GETSN: // Get SN of receiver device at end of link.
if (isReply) // We've been sent an SN for SPort.
{
g.buffer[dataIndex + g.dataLength()] = 0;
Manager.SerialNumberManager.UpdateSerialNumbers(g.buffer, dataIndex, g.dataLength(), g.iNetIf, g.sender());
if (mtx.MsgBuffer.iNetIf != ProtocolConstants.NETIF_USER_BASE) // Notify master of SN via connection attach command.
{
SendLinkMediatorCommand(this, true, ProtocolConstants.MEDIATOR_CONNECT_ATTACH, g.dataLength(), g.buffer, dataIndex);
}
break;
}
// Sender wants to know SN of master.
g.dataLength(Manager.SerialNumberManager.CopyMasterSerialNumber(g.buffer, dataIndex, g.iNetIf));
mtx.ChangeDir = true;
mtx.Finish = MessageTransaction.FinishAction.Normal;
break;
case ProtocolConstants.MEDIATOR_CONNECT_ATTACH: // TODO: Report attach and SN of device.
case ProtocolConstants.MEDIATOR_CONNECT_DETACH: // TODO: Report detach of device.
case ProtocolConstants.MEDIATOR_CONNECT_GATEWAY: // TODO: Register as a gateway.
return(10);
default:
// TODO: try handling via NETIF_MEDIATOR_PORT.
return(11);
}
return(0);
}
return(BabelMessage.ProcessIncomingRawMesssage(Exchange, mtx));
}
// Read parameter values from RAM or EEPROM.
// Command syntax: data[]={ readId, cmdFlags, page, <numRead:>numToRead, {parameterIndexes}+ }
// <[{[VarKind[,StoreFlgs,ArgsFlgs,value data]}*]>.
// Or: data[]={ readId, cmdFlags, page, name} when BX_MEDIATOR_DEVICE_RWV_FLAGS_BY_NAME cmdFlag set.
// In reply to a BY_NAME: strips out request name and sets NAME flag to reply in std form.
// Returns number of values read / changed depending on readFlags:
// BX_MEDIATOR_DEVICE_MONITORVAR = Iso compare and return true if parameters have changed.
// For each parameter, replies with:
// {DSK [,DSF, [ ramValue,] [ eepromValue,] [defaultValue]
// [dataSize, minValue, maxValue] [name + '\0'] ]}.
public byte MediatorReadCommand(MessageTransaction mtx)
{
DeviceParameterTable pt = Exchange.ParameterTable;
DeviceParameter d;
PacketBuffer g = mtx.MsgBuffer;
byte[] pAry;
int numReadIndex, dataIndex;
bool addMeta, addKindOnly;
byte n, j, k, byteCount, numToRead;
byte readId, cmdFlags, pageNum, parameterVarKind, parameterFlags, parameterSize, metaSize;
byte c = 0, maxData = (byte)(Exchange.Manager.MaxPacketSize - ProtocolConstants.GENERAL_OVERHEADS_SIZE);
byte[] paramIndexes = new byte[8];
n = g.dataLength();
if (n < 5)
{
return(0);
}
numReadIndex = -1;
dataIndex = ProtocolConstants.GENERAL_DATA_ARRAY_OFFSET;
pAry = g.buffer;
readId = pAry[dataIndex++];
cmdFlags = pAry[dataIndex++];
pageNum = pAry[dataIndex++];
n -= 3;
// TODO: table page selection.
addKindOnly = (cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_PACK_KIND) != 0;
addMeta = addKindOnly || (cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_PACK) == 0;
if ((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_BY_NAME) != 0)
{
pAry[dataIndex + n] = 0;
string name = Primitives.GetArrayStringValue(pAry, dataIndex, n);
// Search for name in table.
d = pt.Find(name);
if (d == null)
{
pAry[dataIndex - 1] = cmdFlags;
dataIndex += n;
numToRead = 0;
}
else
{
// If found, fall through and add meta data.
cmdFlags |= ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_NAME;
numToRead = 1;
paramIndexes[0] = (byte)d.ParamIndex;
}
}
else
{
--n;
numToRead = (byte)(pAry[dataIndex++] & 0xf);
if (numToRead > n)
{
numToRead = n;
}
j = numToRead;
if (numToRead > 8)
{
numToRead = 8;
}
for (k = 0; k < numToRead; k++)
{
paramIndexes[k] = pAry[dataIndex + k];
}
}
if (numToRead > 0)
{
dataIndex = ProtocolConstants.GENERAL_DATA_ARRAY_OFFSET;
pAry = g.buffer;
pAry[dataIndex++] = readId;
pAry[dataIndex++] = cmdFlags; // Restore parameters.
pAry[dataIndex++] = pageNum;
numReadIndex = dataIndex;
pAry[dataIndex++] = 0;
for (k = 0; k < numToRead; k++)
{
pAry[dataIndex++] = paramIndexes[k];
}
}
byteCount = (byte)dataIndex;
g.dataLength(byteCount);
k = 0;
while (k < numToRead)
{
byte paramIndex = paramIndexes[k];
if (paramIndex > pt.GetLastEntryIndex())
{
break;
}
d = pt.Find(paramIndex);
if (d == null)
{
d = DeviceParameterTable.NullParameter;
}
n = 0;
parameterVarKind = (byte)d.ParamVarKind;
if (addMeta)
{
// Write out representation kind.
metaSize = 1;
if ((byteCount + metaSize) >= maxData)
{
break;
}
pAry[dataIndex + n++] = parameterVarKind;
}
else
{
metaSize = 0;
}
if (parameterVarKind != (byte)VariableKind.VK_None)
{
VariableValue p; byte[] b;
byte argsFlags, argsIndex = 0;
parameterSize = (byte)d.ParamSize;
parameterFlags = (byte)d.ParamStorageFlags;
if (addMeta && !addKindOnly)
{
// Write out parameter flags & space for argsFlags.
metaSize += 2;
if ((byteCount + metaSize) >= maxData)
{
break;
}
pAry[dataIndex + n++] = parameterFlags;
argsIndex = n++;
}
argsFlags = 0;
// Now write out parameter details depending on cmd flags.
if (((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_RAM) != 0) &&
((parameterFlags & (byte)StorageFlags.SF_RAM) != 0))
{
p = d.RamValue;
if (p != null)
{
if (parameterVarKind == (byte)VariableKind.VK_String) // Get size of string.
{
byte maxSize = (byte)d.MaxValue.ValueLong;
int s = p.ValueString.Length;
if (s > maxSize)
{
s = maxSize;
}
parameterSize = (byte)(1 + s); // Allow one for len byte.
}
if ((byteCount + metaSize + parameterSize) < maxData)
{
argsFlags |= ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_RAM;
metaSize += parameterSize;
if (parameterVarKind == (byte)VariableKind.VK_String) // Prefix a len byte.
{
--parameterSize;
pAry[dataIndex + n++] = parameterSize;
b = Primitives.StringToByteArray(p.ValueString);
for (j = 0; j < parameterSize; j++)
{
pAry[dataIndex + n++] = b[j];
}
}
else
{
if ((mtx.TaskCmd != ProtocolConstants.MEDIATOR_DEVICE_ISOMONVAR) || (c != 0) || (paramIndex == ProtocolConstants.PARAMETER_TABLE_INDEX_TICKER))
{
Array.Copy(p.GetBytes(), 0, pAry, dataIndex + n, parameterSize);
}
else
{
int h = parameterSize;
b = p.GetBytes();
if (h > b.Length)
{
h = b.Length;
}
for (j = 0; j < parameterSize; j++)
{
int i = dataIndex + n + j;
if ((c == 0) && pAry[i] != b[j])
{
c = 1;
}
pAry[i] = b[j];
}
}
n += parameterSize;
}
}
}
}
if (((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_EEPROM) != 0) &&
((parameterFlags & (byte)StorageFlags.SF_EEPROM) != 0))
{
p = d.EEPromValue;
if (p != null)
{
if (parameterVarKind == (byte)VariableKind.VK_String) // Get size of string.
{
byte maxSize = (byte)d.MaxValue.ValueLong;
int s = p.ValueString.Length;
if (s > maxSize)
{
s = maxSize;
}
parameterSize = (byte)(1 + s); // Allow one for len byte.
}
if ((byteCount + metaSize + parameterSize) < maxData)
{
argsFlags |= ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_EEPROM;
metaSize += parameterSize;
if (parameterVarKind == (byte)VariableKind.VK_String) // Prefix a len byte.
{
--parameterSize;
pAry[dataIndex + n++] = parameterSize;
b = p.GetBytes();
for (j = 0; j < parameterSize; j++)
{
pAry[dataIndex + n++] = b[j];
}
}
else
{
Array.Copy(p.GetBytes(), 0, pAry, dataIndex + n, parameterSize);
n += parameterSize;
}
}
}
}
if (parameterVarKind == (byte)VariableKind.VK_String)
{
parameterSize = 1;
}
else
{
if ((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_DEFAULT) != 0)
{
if ((byteCount + metaSize + parameterSize) < maxData)
{
argsFlags |= ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_DEFAULT;
metaSize += parameterSize;
Array.Copy(d.DefaultValue.GetBytes(), 0, pAry, dataIndex + n, parameterSize);
n += parameterSize;
}
}
}
if ((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_RANGE) != 0)
{
byte rangeSize = (byte)(2 + parameterSize * 2);
if ((byteCount + metaSize + rangeSize) < maxData)
{
argsFlags |= ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_RANGE;
metaSize += rangeSize;
pAry[dataIndex + n++] = parameterSize;
pAry[dataIndex + n++] = (byte)d.ParamCategory;
Array.Copy(d.MinValue.GetBytes(), 0, pAry, dataIndex + n, parameterSize);
n += parameterSize;
Array.Copy(d.MaxValue.GetBytes(), 0, pAry, dataIndex + n, parameterSize);
n += parameterSize;
}
}
if ((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_NAME) != 0)
{
// Get size of string.
parameterSize = 0;
string s = d.ParamName;
if (s != null)
{
parameterSize = (byte)s.Length;
}
if (parameterSize > ProtocolConstants.MAX_PARAMETER_NAME_SIZE)
{
parameterSize = ProtocolConstants.MAX_PARAMETER_NAME_SIZE;
}
if ((byteCount + metaSize + 1 + parameterSize) < maxData)
{
argsFlags |= ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_NAME;
metaSize += (byte)(1 + parameterSize);
pAry[dataIndex + n++] = parameterSize;
b = Primitives.StringToByteArray(s);
for (j = 0; j < parameterSize; j++)
{
pAry[dataIndex + n++] = b[j];
}
}
}
if (addMeta && !addKindOnly)
{
pAry[dataIndex + argsIndex] = argsFlags;
}
}
byteCount += metaSize;
dataIndex += metaSize;
g.dataLength((byte)(g.dataLength() + metaSize));
++k;
}
if (numReadIndex >= 0)
{
g.buffer[numReadIndex] = (byte)((k << 4) + numToRead);
}
mtx.ChangeDir = true;
mtx.Finish = MessageTransaction.FinishAction.Normal;
if (mtx.TaskCmd == ProtocolConstants.MEDIATOR_DEVICE_ISOMONVAR)
{
return(c);
}
return(k);
}
// Setup an isochronous task.
// Format:
// for ISO[MON]VAR: as per standard ReadCommand,
// but CmdFlags preceded by: ISODetails={IsoId,IsoIntLo,IsoIntHi,RepeatLo,RepeatHi},
// for ISOMSG: <cmd=ISOMSG> ISODetails {msgCmd,...},
// where: IsoInt is the millisecond interval between reads, 0=remove,
// Repeat is the number of times to send data, 0=forever.
// Id for deleting task is SenderParams.
int MediatorIsoCommand(MessageTransaction mtx)
{
PacketBuffer g = mtx.MsgBuffer;
IsoTask t;
byte[] pAry;
int dataIndex;
int pIndex;
uint msInterval, repeatCount;
byte n, k, isoId;
bool isIsoMsg;
n = g.dataLength();
if (n < 3)
{
return(0);
}
pAry = g.buffer;
pIndex = dataIndex = ProtocolConstants.GENERAL_DATA_ARRAY_OFFSET;
isoId = pAry[dataIndex++];
msInterval = pAry[dataIndex++];
msInterval += (uint)(pAry[dataIndex++] << 8);
// First check if this is a cancel or update request.
// Find isoTask, if it exists, using isoId + senderParams as id.
for (k = 0; k < NumTasks; k++)
{
do
{
byte j;
t = IsoTasks[IsoTaskQueue[k]];
if (t.isoId == isoId &&
t.addressParams.Sender == g.sender() &&
t.addressParams.SenderId == g.senderId() &&
t.addressParams.flagsRS == (g.flagsRS() & ProtocolConstants.MESSAGE_PORTS_MASK))
{
// Free message buffer.
Manager.IoBuffersFreeHeap.Release(t.ioIndex);
t.ioIndex = -1;
// Remove from task queue.
for (j = k; j < (NumTasks - 1); j++)
{
IsoTaskQueue[j] = IsoTaskQueue[j + 1];
}
--NumTasks;
if (k < NumTasks)
{
continue;
}
}
break;
} while (true);
}
if (msInterval == 0)
{
return(0);
}
isIsoMsg = (g.command() == ProtocolConstants.MEDIATOR_DEVICE_ISOMSG);
if (isIsoMsg)
{
if (n < 6)
{
return(0); // Must be at least 5 iso parameters + 1 cmd byte.
}
}
else if (n < 8)
{
return(0); // Must be at least 5 iso parameter bytes + 3 read bytes.
}
// Set up a new isoTask.
// First check if a task slot is free.
if (NumTasks == IsoTaskSize)
{
return(0);
}
t = null;
for (k = 0; k < IsoTaskSize; k++)
{
if (IsoTasks[k].ioIndex == -1)
{
t = IsoTasks[k];
break;
}
}
if (t == null)
{
return(0);
}
// Second, get iso params.
repeatCount = pAry[dataIndex++];
repeatCount += (uint)(pAry[dataIndex++] << 8);
t.isoId = isoId;
t.repeatCount = repeatCount;
t.msTicksRelativeToGo = 0; // Schedule for immediate execution because inserted at head of Q.
t.msInterval = msInterval;
t.ioIndex = mtx.IoIndex;
t.state = 0;
t.cmd = g.command(); // Original msg iso cmd.
t.ownerDriver = (isIsoMsg ? Manager.GetLinkDriver(ProtocolConstants.NETIF_MEDIATOR_PORT) : this); // For IsoMsg, needs to be MediatorNetIf.
t.addressParams.SenderId = g.senderId();
t.addressParams.Sender = g.sender();
t.addressParams.flagsRS = (byte)(g.flagsRS() & ProtocolConstants.MESSAGE_PORTS_MASK);
// Now convert iso to a regular read message.
// For ISOMSG cmd, g->command needs to be pulled from next dataAry byte.
if (isIsoMsg)
{
g.command(pAry[dataIndex++]);
--n;
}
else
{
g.command(ProtocolConstants.MEDIATOR_DEVICE_READVAR);
}
n -= 5;
g.dataLength(n);
while (n-- > 0)
{
pAry[pIndex++] = pAry[dataIndex++];
}
mtx.Finish = MessageTransaction.FinishAction.Keep; // Keep iobuffer.
// Finally insert task at head of queue.
for (n = (byte)NumTasks; n > 0; n--)
{
IsoTaskQueue[n] = IsoTaskQueue[n - 1];
}
IsoTaskQueue[0] = k;
++NumTasks;
return(1);
}
// Write parameter values to RAM or EEPROM.
// Writes default value if no value given.
// Command syntax: data[]={writeId, cmdFlags, page, <numWritten:>numToWrite, {parameterIndex}+[data].
// Returns number written along with parameter Index's.
byte MediatorWriteCommand(MessageTransaction mtx)
{
DeviceParameterTable pt = Exchange.ParameterTable;
DeviceParameter d;
PacketBuffer g = mtx.MsgBuffer;
byte[] pAry;
int dataIndex;
VariableValue val = null;
byte n, h, k, j, writeId, cmdFlags, pageNum, retArgsLen, maxSize = 0;
VariableKind parameterVarKind;
StorageFlags parameterFlags;
byte parameterSize, numToWrite;
int paramIndexes, numWrittenIndex;
n = g.dataLength();
if (n < 5)
{
return(0);
}
dataIndex = ProtocolConstants.GENERAL_DATA_ARRAY_OFFSET;
pAry = g.buffer;
writeId = pAry[dataIndex++];
cmdFlags = pAry[dataIndex++];
pageNum = pAry[dataIndex++];
numWrittenIndex = dataIndex;
numToWrite = (byte)(pAry[dataIndex++] & 0xf);
n -= 4;
if (numToWrite > n)
{
numToWrite = n;
}
j = numToWrite;
if (numToWrite > 8)
{
numToWrite = 8;
}
paramIndexes = dataIndex;
retArgsLen = (byte)(dataIndex + numToWrite);
g.dataLength((byte)(g.dataLength() - (4 + j))); //3=(wId, cmdFlags, page, nW:nTW), j= num original idx's.
dataIndex += j;
h = 0; k = 0;
while (k < numToWrite)
{
int paramIndex = pAry[paramIndexes + k];
if (paramIndex > pt.GetLastEntryIndex())
{
break;
}
d = pt.Find(paramIndex);
if (d == null)
{
d = DeviceParameterTable.NullParameter;
}
parameterSize = (byte)d.ParamSize;
if (parameterSize != 0)
{
int valPtrIndex = 0;
parameterVarKind = (VariableKind)d.ParamVarKind;
parameterFlags = (StorageFlags)d.ParamStorageFlags;
if (parameterVarKind == VariableKind.VK_String) // Note: only one string allowed per message.
{
parameterSize = 0;
if (n > 0) // Strings must be at least 1 byte long: len byte.
{
byte lenAdj;
maxSize = (byte)d.MaxValue.ValueLong;
parameterSize = pAry[dataIndex++];
lenAdj = parameterSize;
n--;
if (parameterSize > maxSize)
{
parameterSize = maxSize;
}
if (n < parameterSize)
{
parameterSize = n;
lenAdj = parameterSize;
}
valPtrIndex = dataIndex;
n -= lenAdj;
dataIndex += lenAdj;
}
}
else
{
if (n >= parameterSize)
{
val = new VariableValue(parameterVarKind, pAry, dataIndex, (int)parameterSize);
valPtrIndex = dataIndex;
n -= parameterSize;
dataIndex += parameterSize;
}
else
{
val = d.DefaultValue;
valPtrIndex = -1;
}
val.CheckInRange(d.MinValue, d.MaxValue);
}
if (parameterSize > 0)
{
if ((parameterFlags & StorageFlags.SF_ReadOnly) == 0)
{
if (((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_EEPROM) != 0) &&
((parameterFlags & StorageFlags.SF_EEPROM) != 0))
{
if (parameterVarKind == VariableKind.VK_String)
{
if (valPtrIndex >= 0)
{
d.EEPromValue = new VariableValue(parameterVarKind, pAry, valPtrIndex, (int)parameterSize);
}
else
{
d.EEPromValue = new VariableValue(parameterVarKind, pAry, 0, (int)0);
}
}
else
{
d.EEPromValue = val;
}
}
if ((cmdFlags & ProtocolConstants.MEDIATOR_DEVICE_RWV_FLAGS_RAM) != 0)
{
if (parameterVarKind == VariableKind.VK_String)
{
if (valPtrIndex >= 0)
{
d.RamValue = new VariableValue(parameterVarKind, pAry, valPtrIndex, (int)parameterSize);
}
else
{
d.RamValue = new VariableValue(parameterVarKind, pAry, 0, (int)0);
}
}
else
{
d.RamValue = val;
}
}
}
}
++h;
}
++k;
++paramIndex;
}
if (numWrittenIndex >= 0)
{
pAry[numWrittenIndex] = h; // Reply to caller with number of parameters written.
}
g.dataLength(retArgsLen);
mtx.ChangeDir = true;
mtx.Finish = ((g.flagsRS() & ProtocolConstants.MESSAGE_FLAGS_ACK) != 0)
? MessageTransaction.FinishAction.Normal : MessageTransaction.FinishAction.Free;
return(h);
}