unsigned __int8 PrologEncoderMD::GetXmmRegToSave(IR::Instr *instr, unsigned __int16 *scaledOffset)
{
Assert(scaledOffset);
Assert(instr->m_opcode == Js::OpCode::MOVAPD || instr->m_opcode == Js::OpCode::MOVAPS);
Assert(instr->GetDst() && instr->GetDst()->IsIndirOpnd());
unsigned __int8 reg = ((instr->GetSrc1()->AsRegOpnd()->GetReg() - FIRST_XMM_REG) & 0xFF);
unsigned __int32 offsetFromInstr = instr->GetDst()->AsIndirOpnd()->GetOffset();
// The offset in the instruction is relative to the stack pointer before the saved reg size and stack args size were
// subtracted, but the offset in the unwind info needs to be relative to the final stack pointer value
// (see LowererMDArch::LowerEntryInstr), so adjust for that
Assert(instr->GetDst()->AsIndirOpnd()->GetBaseOpnd()->GetReg() == LowererMDArch::GetRegStackPointer());
Func *const topFunc = instr->m_func->GetTopFunc();
offsetFromInstr += topFunc->GetArgsSize() + topFunc->GetSavedRegSize();
// Can only encode nonnegative 16-byte-aligned offsets in the unwind info
Assert(static_cast<int32>(offsetFromInstr) >= 0);
Assert(::Math::Align(offsetFromInstr, static_cast<unsigned __int32>(MachDouble * 2)) == offsetFromInstr);
// Stored offset is scaled by 16
offsetFromInstr /= MachDouble * 2;
// We currently don't allow a total stack size greater than 1 MB. If we start allowing that, we will need to handle offsets
// greater than (1 MB - 16) in the unwind info as well, for which a FAR version of the op-code is necessary.
Assert(IS_UINT16(offsetFromInstr));
*scaledOffset = TO_UINT16(offsetFromInstr);
return reg;
}
/**
* Handle Modbus requests
* Checks for new TCP data and looks for new modbus messages
**/
void mb_handleRequest(void) {
// read packet, handle ping and wait for a tcp packet
uint16_t dat_p = packetloop_icmp_tcp(buf,enc28j60PacketReceive(BUFFER_SIZE, buf));
// check if data was received
if (dat_p == 0)
return;
// make structure pointer to buffer
modbusmsg_t* msg = (modbusmsg_t*)&buf[dat_p];
// check for modbus protocol id
if (FROM_UINT16(msg->mbap.protoId) > 0)
return;
uint16_t len;
switch (msg->function) {
// Modbus read function
case MB_FUNC_READREG: {
readreg_req_t* req = (readreg_req_t*)&(msg->function);
readreg_res_t* res = (readreg_res_t*)&(msg->function);
uint16_t amount = FROM_UINT16(req->amount);
uint16_t addr = FROM_UINT16(req->start_address);
if (amount == 0 || amount > 0x7D) {
len = mb_set_error(msg,MB_ERR_DATA);
break;
}
if (addr + amount <= MODBUS_REGISTER_COUNT) {
uint16_t *ptr = (uint16_t*)&(res->function);
uint16_t cnt = 0;
for (; cnt < amount; cnt++) {
if (registers[addr+cnt] == NULL) {
len = mb_set_error(msg,MB_ERR_PROC);
break;
}
++ptr;
*ptr = TO_UINT16(*(registers[addr+cnt]));
}
// if an error occurred
if (cnt < amount)
break;
res->bytecount = cnt*sizeof(uint16_t);
len = sizeof(readreg_res_t)+cnt*sizeof(uint16_t);
} else
len = mb_set_error(msg,MB_ERR_ADDR);
break;
}
// Modbus write function
case MB_FUNC_WRITEREG: {
writereg_req_t* req = (writereg_req_t*)&(msg->function);
uint16_t addr = FROM_UINT16(req->start_address);
uint16_t value = FROM_UINT16(req->value);
if (addr >= MODBUS_REGISTER_COUNT || writecbs[addr] == NULL) {
len = mb_set_error(msg,MB_ERR_ADDR);
break;
}
if (writecbs[addr](addr,value))
len = sizeof(writereg_req_t);
else
len = mb_set_error(msg,MB_ERR_PROC);
break;
}
// Modbus device id function
// Not implemented completely
case MB_FUNC_DEV_ID: {
devid_req_t* req = (devid_req_t*)&(msg->function);
devid_res_t* res = (devid_res_t*)&(msg->function);
devid_obj_t* obj = (devid_obj_t*)&(res->nextobjid);
uint8_t i = req->objid;
if (req->type != 0x0E) {
len = mb_set_error(msg,MB_ERR_FUNC);
break;
}
if (!req->code || req->code > 4) {
len = mb_set_error(msg,MB_ERR_DATA);
break;
}
res->conflevel = 0x01;
res->more = 0x00;
res->nextobjid = 0x00;
res->amount = 3;
len = sizeof(devid_res_t);
if (i > 2)
i = 0;
// add requested objects
for (; i < 3; i++) {
++obj;
obj->id = i;
obj->length = 0;
len += sizeof(devid_obj_t)+obj->length;
}
break;
}
default: // set error
len = mb_set_error(msg,MB_ERR_FUNC);
break;
}
msg->mbap.length = TO_UINT16(len+1);
// send response
www_server_reply(buf,sizeof(mbap_t)+len);
}
/** Process a general service request message.
*
* @param[in] message - The message to process.
* @param[out] sendbuf - The response buffer to fill.
* @param[in] errorcode - The error code from the client request.
*
* @return Zero on success, or a non-zero SLP error on failure.
*
* @internal
*/
static int ProcessSrvRqst(SLPMessage * message, SLPBuffer * sendbuf,
int errorcode)
{
int i;
SLPUrlEntry * urlentry;
SLPDDatabaseSrvRqstResult * db = 0;
size_t size = 0;
SLPBuffer result = *sendbuf;
#ifdef ENABLE_SLPv2_SECURITY
SLPAuthBlock * authblock = 0;
int j;
#endif
/* If errorcode is set, we can not be sure that message is good
Go directly to send response code
*/
if (errorcode)
goto RESPOND;
/* Check for one of our IP addresses in the prlist */
if (SLPIntersectStringList(message->body.srvrqst.prlistlen,
message->body.srvrqst.prlist, G_SlpdProperty.interfacesLen,
G_SlpdProperty.interfaces))
{
/* silently ignore */
result->end = result->start;
goto FINISHED;
}
/* Make sure that we handle at least verify registrations made with
the requested SPI. If we can't then have to return an error
because there is no way we can return URL entries that are
signed in a way the requester can understand
*/
#ifdef ENABLE_SLPv2_SECURITY
if (G_SlpdProperty.securityEnabled)
{
if (SLPSpiCanVerify(G_SlpdSpiHandle, message->body.srvrqst.spistrlen,
message->body.srvrqst.spistr) == 0)
{
errorcode = SLP_ERROR_AUTHENTICATION_UNKNOWN;
goto RESPOND;
}
}
else if (message->body.srvrqst.spistrlen)
{
errorcode = SLP_ERROR_AUTHENTICATION_UNKNOWN;
goto RESPOND;
}
#else
if (message->body.srvrqst.spistrlen)
{
errorcode = SLP_ERROR_AUTHENTICATION_UNKNOWN;
goto RESPOND;
}
#endif
/* check to to see if a this is a special SrvRqst */
if (SLPCompareString(message->body.srvrqst.srvtypelen,
message->body.srvrqst.srvtype, 23, SLP_DA_SERVICE_TYPE) == 0)
{
errorcode = ProcessDASrvRqst(message, sendbuf, errorcode);
if (errorcode == 0)
{
/* Since we have an errorcode of 0, we were successful,
and have already formed a response packet; return now.
*/
return errorcode;
}
goto RESPOND;
}
if (SLPCompareString(message->body.srvrqst.srvtypelen,
message->body.srvrqst.srvtype, 21, SLP_SA_SERVICE_TYPE) == 0)
{
errorcode = ProcessSASrvRqst(message, sendbuf, errorcode);
if (errorcode == 0)
{
/* Since we have an errorcode of 0, we were successful,
and have already formed a response packet; return now.
*/
return errorcode;
}
goto RESPOND;
}
/* make sure that we handle the scope */
if (SLPIntersectStringList(message->body.srvrqst.scopelistlen,
message->body.srvrqst.scopelist, G_SlpdProperty.useScopesLen,
G_SlpdProperty.useScopes) != 0)
errorcode = SLPDDatabaseSrvRqstStart(message, &db);
else
errorcode = SLP_ERROR_SCOPE_NOT_SUPPORTED;
RESPOND:
/* do not send error codes or empty replies to multicast requests */
if (errorcode != 0 || db->urlcount == 0)
{
if (message->header.flags & SLP_FLAG_MCAST
|| SLPNetIsMCast(&(message->peer)))
{
result->end = result->start;
goto FINISHED;
}
}
/* ensure the buffer is big enough to handle the whole srvrply */
size = message->header.langtaglen + 18;/* 14 bytes for header */
/* 2 bytes for error code */
/* 2 bytes for url count */
if (errorcode == 0)
{
for (i = 0; i < db->urlcount; i++)
{
/* urlentry is the url from the db result */
urlentry = db->urlarray[i];
size += urlentry->urllen + 6; /* 1 byte for reserved */
/* 2 bytes for lifetime */
/* 2 bytes for urllen */
/* 1 byte for authcount */
#ifdef ENABLE_SLPv2_SECURITY
/* make room to include the authblock that was asked for */
if (G_SlpdProperty.securityEnabled
&& message->body.srvrqst.spistrlen)
{
for (j = 0; j < urlentry->authcount; j++)
{
if (SLPCompareString(urlentry->autharray[j].spistrlen,
urlentry->autharray[j].spistr,
message->body.srvrqst.spistrlen,
message->body.srvrqst.spistr) == 0)
{
authblock = &(urlentry->autharray[j]);
size += authblock->length;
break;
}
}
}
#endif
}
}
/* reallocate the result buffer */
result = SLPBufferRealloc(result, size);
if (result == 0)
{
errorcode = SLP_ERROR_INTERNAL_ERROR;
goto FINISHED;
}
/* add the header */
/* version */
*result->curpos++ = 2;
/* function id */
*result->curpos++ = SLP_FUNCT_SRVRPLY;
/* length */
PutUINT24(&result->curpos, size);
/* flags */
PutUINT16(&result->curpos, (size > (size_t)G_SlpdProperty.MTU?
SLP_FLAG_OVERFLOW: 0));
/* ext offset */
PutUINT24(&result->curpos, 0);
/* xid */
PutUINT16(&result->curpos, message->header.xid);
/* lang tag len */
PutUINT16(&result->curpos, message->header.langtaglen);
/* lang tag */
memcpy(result->curpos, message->header.langtag,
message->header.langtaglen);
result->curpos += message->header.langtaglen;
/* add rest of the SrvRply */
/* error code*/
PutUINT16(&result->curpos, errorcode);
if (errorcode == 0)
{
/* urlentry count */
PutUINT16(&result->curpos, db->urlcount);
for (i = 0; i < db->urlcount; i++)
{
/* urlentry is the url from the db result */
urlentry = db->urlarray[i];
#ifdef ENABLE_SLPv1
if (urlentry->opaque == 0)
{
/* url-entry reserved */
*result->curpos++ = 0;
/* url-entry lifetime */
PutUINT16(&result->curpos, urlentry->lifetime);
/* url-entry urllen */
PutUINT16(&result->curpos, urlentry->urllen);
/* url-entry url */
memcpy(result->curpos, urlentry->url, urlentry->urllen);
result->curpos += urlentry->urllen;
/* url-entry auths */
*result->curpos++ = 0;
}
else
#endif
{
/* Use an opaque copy if available (and authentication is
* not being used).
*/
/* TRICKY: Fix up the lifetime. */
TO_UINT16(urlentry->opaque + 1, urlentry->lifetime);
memcpy(result->curpos, urlentry->opaque, urlentry->opaquelen);
result->curpos += urlentry->opaquelen;
}
}
}
else
PutUINT16(&result->curpos, 0); /* set urlentry count to 0*/
FINISHED:
if (db)
SLPDDatabaseSrvRqstEnd(db);
*sendbuf = result;
return errorcode;
}
/** Process a DA service request message.
*
* @param[in] message - The message to process.
* @param[out] sendbuf - The response buffer to fill.
* @param[in] errorcode - The error code from the client request.
*
* @return Zero on success, or a non-zero SLP error on failure.
*
* @internal
*/
static int ProcessDASrvRqst(SLPMessage * message, SLPBuffer * sendbuf, int errorcode)
{
SLPBuffer tmp = 0;
SLPMessage * msg = 0;
void * eh = 0;
/* TODO should really be a configurable property, maybe G_SlpdProperty.MTU? Left at 4096 to retain same behaviour */
size_t initial_buffer_size = 4096;
size_t grow_size = initial_buffer_size;
/* Special case for when libslp asks slpd (through the loopback) about
a known DAs. Fill sendbuf with DAAdverts from all known DAs.
*/
if (SLPNetIsLoopback(&message->peer))
{
*sendbuf = SLPBufferRealloc(*sendbuf, initial_buffer_size);
if (*sendbuf == 0)
return SLP_ERROR_INTERNAL_ERROR;
if (errorcode == 0)
{
/* Note: The weird *sendbuf code is making a single SLPBuffer
that contains multiple DAAdverts. This is a special
process that only happens for the DA SrvRqst through
loopback to the SLPAPI
*/
/*If we are a DA, always have ourself at the start of the list, so the
lib requests can be handled locally for speed */
if(G_SlpdProperty.isDA)
{
struct sockaddr_storage loaddr;
if(SLPNetIsIPV4())
{
int addr = INADDR_LOOPBACK;
SLPNetSetAddr(&loaddr, AF_INET, G_SlpdProperty.port, &addr);
}
else
{
SLPNetSetAddr(&loaddr, AF_INET6, G_SlpdProperty.port, &slp_in6addr_loopback);
}
if(0 == SLPDKnownDAGenerateMyDAAdvert(&loaddr, 0, 0, 0, message->header.xid, &tmp))
{
memcpy((*sendbuf)->curpos, tmp->start, tmp->end - tmp->start);
(*sendbuf)->curpos = ((*sendbuf)->curpos) + (tmp->end - tmp->start);
SLPBufferFree(tmp);
tmp = 0;
}
else
{
SLPDLog("Unable to add initial DAAdvert due to error\n");
}
}
eh = SLPDKnownDAEnumStart();
if (eh)
{
while (1)
{
/* iterate over all database entries */
if (SLPDKnownDAEnum(eh, &msg, &tmp) == 0)
{
break;
}
/* if we resize succesfully.. */
if( CheckAndResizeBuffer(sendbuf, tmp, grow_size) == 0 )
{
/* buffer should now be resized to an appropriate size to handle all current database entries */
/* TRICKY: Fix up the XID and clear the flags. */
tmp->curpos = tmp->start + 10;
TO_UINT16(tmp->curpos, message->header.xid);
if (*(tmp->start) == 1)
*(tmp->start + 4) = 0;
else
TO_UINT16(tmp->start + 5, 0);
/* copy all data out of tmp into the sendbuf */
memcpy((*sendbuf)->curpos, tmp->start, tmp->end - tmp->start);
/* increment the current position in sendbuf */
(*sendbuf)->curpos = ((*sendbuf)->curpos) + (tmp->end - tmp->start);
}
else
{
errorcode = SLP_ERROR_INTERNAL_ERROR;
}
}
SLPDKnownDAEnumEnd(eh);
}
/* tmp can store database entries which should not be freed by anyone else so reset the pointer to prevent double deletion */
tmp = 0;
/* Tack on a "terminator" DAAdvert
Note that this function *always* returns the error code passed as its second parameter (or SLP_ERROR_INTERNAL_ERROR if the buffer fails to resize)
The errorcode is also inserted into the srvrply header by this function
*/
SLPDKnownDAGenerateMyDAAdvert(&message->localaddr, SLP_ERROR_INTERNAL_ERROR, 0, 0, message->header.xid, &tmp);
/* if we resize succesfully.. */
if ( CheckAndResizeBuffer(sendbuf, tmp, grow_size) == 0 )
{
memcpy((*sendbuf)->curpos, tmp->start, tmp->end - tmp->start);
(*sendbuf)->curpos = ((*sendbuf)->curpos) + (tmp->end - tmp->start);
/* mark the end of the sendbuf */
(*sendbuf)->end = (*sendbuf)->curpos;
}
else
{
errorcode = SLP_ERROR_INTERNAL_ERROR;
}
SLPBufferFree(tmp);
tmp = 0;
}
return errorcode;
}
/* Normal case where a remote Agent asks for a DA */
*sendbuf = SLPBufferRealloc(*sendbuf, G_SlpdProperty.MTU);
if (*sendbuf == 0)
return SLP_ERROR_INTERNAL_ERROR;
if (G_SlpdProperty.isDA)
{
if (message->body.srvrqst.scopelistlen == 0
|| SLPIntersectStringList(message->body.srvrqst.scopelistlen,
message->body.srvrqst.scopelist, G_SlpdProperty.useScopesLen,
G_SlpdProperty.useScopes))
{
errorcode = SLPDKnownDAGenerateMyDAAdvert(&message->localaddr,
errorcode, 0, 0, message->header.xid, sendbuf);
}
else
errorcode = SLP_ERROR_SCOPE_NOT_SUPPORTED;
}
else
errorcode = SLP_ERROR_MESSAGE_NOT_SUPPORTED;
/* don't return errorcodes to multicast messages */
if (errorcode != 0)
{
if (message->header.flags & SLP_FLAG_MCAST
|| SLPNetIsMCast(&(message->peer)))
(*sendbuf)->end = (*sendbuf)->start;
}
return errorcode;
}