static void _start_process(int err, seL4_Word pc, void *cookie) {
process_create_data *data = (process_create_data *) cookie;
if (err) {
_fail(err, data);
return;
}
seL4_UserContext context;
dprintf(5, "\tAttaching stdout and stderr...\n");
err = _create_stderrout(data->pcb);
if (err) {
_fail(err, data);
return;
}
dprintf(5, "\tSetting initial program counter...\n");
// Set initial seL4 thread context, starting the process
memset(&context, 0, sizeof(context));
context.pc = pc;
context.sp = SOS_PROCESS_STACK_TOP;
seL4_TCB_WriteRegisters(data->pcb->tcb_cap, 1, 0, 2, &context);
data->pcb->running = 1;
// Call back
_reply(SOS_PROCESS_SUCCESS, data->pcb->pid, data->cb, data->cookie);
_num_procs++;
free(data);
return;
}
static void _fail(int err, process_create_data *proc) {
_reply(err, -1, proc->cb, proc->cookie);
if (proc->pcb) {
proc->pcb->abort = 1;
process_delete(proc->pcb, NULL, NULL);
}
free(proc);
}
size_t MDNSResponder::advertiseServices(){
MDNSService* servicePtr;
size_t i = 0;
for (servicePtr = _services; servicePtr; servicePtr = servicePtr->_next) {
if(servicePtr->_port > 0){
_reply(0x0F, servicePtr->_name, servicePtr->_proto, servicePtr->_port);
i++;
}
}
return i;
}
/* Create a new process from CPIO */
int process_create(char *app_name, process_create_cb cb, void *cookie) {
int err;
dprintf(5, "abcde\n");
dprintf(5, "process_create: %.128s\n", app_name);
process_create_data *data = malloc(sizeof(struct process_create_data));
if (data == NULL) {
dprintf(6, "process_create: OOM\n");
_reply(SOS_PROCESS_OOM, -1, cb, cookie);
return SOS_PROCESS_OOM;
}
dprintf(8, "bzero procreate data\n");
bzero(data, sizeof(struct process_create_data));
dprintf(8, "bzero procreate data done\n");
data->pcb = malloc(sizeof(struct sos_pcb));
if (data->pcb == NULL) {
dprintf(5, "process_create: could not allocate for PCB\n");
_fail(SOS_PROCESS_OOM, data);
return SOS_PROCESS_OOM;
} else {
dprintf(6, "process_create: PCB is at %p\n", data->pcb);
}
bzero(data->pcb, sizeof(struct sos_pcb));
data->pcb->stime = time_stamp();
dprintf(6, "\nCopying app name...\n");
strncpy(data->pcb->app_name, app_name, SOS_PATH_MAX);
data->cb = cb;
data->cookie = cookie;
dprintf(3, "\nStarting \"%s\"...\n", app_name);
dprintf(5, "\tSetting PID...\n");
err = _set_pid(data);
if (err) {
dprintf(6, "Could not get PID\n");
_fail(err, data);
return err;
}
dprintf(5, "\tCreating SOS page directory...\n");
err = _create_sos_pd(data);
if (err) {
_fail(err, data);
return err;
}
// The rest happens in CPS
return SOS_PROCESS_SUCCESS;
}
void Web_listen(Web_t self){
char input[8000];
HttpRequest_t httpRequest = HttpRequest_new();
while(1) {
socket_t * clientSock = socket_accept(self->serverSock);
socket_read(clientSock, input, sizeof(input));
HttpRequest_parse(httpRequest, input);
_reply(self, httpRequest, clientSock);
socket_free(clientSock);
}
}
int t_reply_unsafe( struct cell *t, struct sip_msg* p_msg, unsigned int code,
str * text )
{
return _reply( t, p_msg, code, text, 0 /* don't lock replies */ );
}
int t_reply( struct cell *t, struct sip_msg* p_msg, unsigned int code,
str * text )
{
return _reply( t, p_msg, code, text, 1 /* lock replies */ );
}
INT32 _mongoSession::run()
{
INT32 rc = SDB_OK ;
BOOLEAN bigEndian = FALSE ;
UINT32 msgSize = 0 ;
UINT32 headerLen = sizeof( mongoMsgHeader ) - sizeof( INT32 ) ;
INT32 bodyLen = 0 ;
engine::pmdEDUMgr *pmdEDUMgr = NULL ;
CHAR *pBuff = NULL ;
const CHAR *pBody = NULL ;
const CHAR *pInMsg = NULL ;
engine::monDBCB *mondbcb = engine::pmdGetKRCB()->getMonDBCB() ;
if ( !_pEDUCB )
{
rc = SDB_SYS ;
goto error ;
}
pmdEDUMgr = _pEDUCB->getEDUMgr() ;
bigEndian = checkBigEndian() ;
while ( !_pEDUCB->isDisconnected() && !_socket.isClosed() )
{
_pEDUCB->resetInterrupt() ;
_pEDUCB->resetInfo( engine::EDU_INFO_ERROR ) ;
_pEDUCB->resetLsn() ;
rc = recvData( (CHAR*)&msgSize, sizeof(UINT32) ) ;
if ( rc )
{
if ( SDB_APP_FORCED != rc )
{
PD_LOG( PDERROR, "Session[%s] failed to recv msg size, "
"rc: %d", sessionName(), rc ) ;
}
break ;
}
if ( bigEndian )
{
}
if ( msgSize < headerLen || msgSize > SDB_MAX_MSG_LENGTH )
{
PD_LOG( PDERROR, "Session[%s] recv msg size[%d] is less than "
"mongoMsgHeader size[%d] or more than max msg size[%d]",
sessionName(), msgSize, sizeof( mongoMsgHeader ),
SDB_MAX_MSG_LENGTH ) ;
rc = SDB_INVALIDARG ;
break ;
}
else
{
pBuff = getBuff( msgSize + 1 ) ;
if ( !pBuff )
{
rc = SDB_OOM ;
break ;
}
*(UINT32*)pBuff = msgSize ;
rc = recvData( pBuff + sizeof(UINT32), msgSize - sizeof(UINT32) ) ;
if ( rc )
{
if ( SDB_APP_FORCED != rc )
{
PD_LOG( PDERROR, "Session[%s] failed to recv rest msg, rc: %d",
sessionName(), rc ) ;
}
break ;
}
pBuff[ msgSize ] = 0 ;
{
_resetBuffers() ;
_converter.loadFrom( pBuff, msgSize ) ;
rc = _converter.convert( _inBuffer ) ;
if ( SDB_OK != rc && SDB_OPTION_NOT_SUPPORT != rc)
{
goto error ;
}
_pEDUCB->incEventCount() ;
mondbcb->addReceiveNum() ;
if ( SDB_OK != ( rc = pmdEDUMgr->activateEDU( _pEDUCB ) ) )
{
PD_LOG( PDERROR, "Session[%s] activate edu failed, rc: %d",
sessionName(), rc ) ;
goto error ;
}
if ( _preProcessMsg( _converter.getParser(),
_resource, _contextBuff ) )
{
goto reply ;
}
pInMsg = _inBuffer.data() ;
while ( NULL != pInMsg )
{
rc = _processMsg( pInMsg ) ;
if ( SDB_OK == rc )
{
_authed = TRUE ;
}
rc = _converter.reConvert( _inBuffer, &_replyHeader ) ;
if ( SDB_OK != rc )
{
goto reply ;
}
else
{
if ( !_inBuffer.empty() )
{
_contextBuff.release() ;
pInMsg = _inBuffer.data() ;
}
else
{
pInMsg = NULL ;
}
}
}
reply:
_handleResponse( _converter.getOpType(), _contextBuff ) ;
pBody = _contextBuff.data() ;
bodyLen = _contextBuff.size() ;
INT32 rcTmp = _reply( &_replyHeader, pBody, bodyLen ) ;
if ( rcTmp )
{
PD_LOG( PDERROR, "Session[%s] failed to send response,"
"rc: %d", sessionName(), rcTmp ) ;
goto error ;
}
pBody = NULL ;
bodyLen = 0 ;
_contextBuff.release() ;
if ( SDB_OK != ( rc = pmdEDUMgr->waitEDU( _pEDUCB ) ) )
{
PD_LOG( PDERROR, "Session[%s] wait edu failed, rc: %d",
sessionName(), rc ) ;
goto error ;
}
}
}
} // end while
done:
disconnect() ;
return rc ;
error:
goto done ;
}
/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(Request *req)
{
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
ErrorCode errorCode = kErrNone;
// basic sanity checks; must validate length before use
if (payload->size > kMaxDataLength) {
errorCode = kErrInvalidDataSize;
goto out;
}
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
#endif
switch (payload->opcode) {
case kCmdNone:
break;
case kCmdTerminateSession:
errorCode = _workTerminate(payload);
break;
case kCmdResetSessions:
errorCode = _workReset(payload);
break;
case kCmdListDirectory:
errorCode = _workList(payload);
break;
case kCmdOpenFileRO:
errorCode = _workOpen(payload, O_RDONLY);
break;
case kCmdCreateFile:
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
break;
case kCmdOpenFileWO:
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
break;
case kCmdReadFile:
errorCode = _workRead(payload);
break;
case kCmdWriteFile:
errorCode = _workWrite(payload);
break;
case kCmdRemoveFile:
errorCode = _workRemoveFile(payload);
break;
case kCmdRename:
errorCode = _workRename(payload);
break;
case kCmdTruncateFile:
errorCode = _workTruncateFile(payload);
break;
case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload);
break;
case kCmdRemoveDirectory:
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
default:
errorCode = kErrUnknownCommand;
break;
}
out:
// handle success vs. error
if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode;
payload->opcode = kRspAck;
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: ack\n");
#endif
} else {
int r_errno = errno;
warnx("FTP: nak %u", errorCode);
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
payload->size = 1;
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = r_errno;
}
}
// respond to the request
_reply(req);
_return_request(req);
}
void
MavlinkFTP::_worker(Request *req)
{
auto hdr = req->header();
ErrorCode errorCode = kErrNone;
uint32_t messageCRC;
// basic sanity checks; must validate length before use
if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
errorCode = kErrNoRequest;
goto out;
}
// check request CRC to make sure this is one of ours
messageCRC = hdr->crc32;
hdr->crc32 = 0;
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
errorCode = kErrNoRequest;
goto out;
printf("ftp: bad crc\n");
}
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
switch (hdr->opcode) {
case kCmdNone:
break;
case kCmdTerminate:
errorCode = _workTerminate(req);
break;
case kCmdReset:
errorCode = _workReset();
break;
case kCmdList:
errorCode = _workList(req);
break;
case kCmdOpen:
errorCode = _workOpen(req, false);
break;
case kCmdCreate:
errorCode = _workOpen(req, true);
break;
case kCmdRead:
errorCode = _workRead(req);
break;
case kCmdWrite:
errorCode = _workWrite(req);
break;
case kCmdRemove:
errorCode = _workRemove(req);
break;
default:
errorCode = kErrNoRequest;
break;
}
out:
// handle success vs. error
if (errorCode == kErrNone) {
hdr->opcode = kRspAck;
printf("FTP: ack\n");
} else {
printf("FTP: nak %u\n", errorCode);
hdr->opcode = kRspNak;
hdr->size = 1;
hdr->data[0] = errorCode;
}
// respond to the request
_reply(req);
// free the request buffer back to the freelist
_qFree(req);
}
void MDNSResponder::_parsePacket(){
int i;
char tmp;
bool serviceParsed = false;
bool protoParsed = false;
bool localParsed = false;
char hostName[255];
uint8_t hostNameLen;
char serviceName[32];
uint8_t serviceNameLen;
uint16_t servicePort = 0;
char protoName[32];
protoName[0] = 0;
uint8_t protoNameLen = 0;
uint16_t packetHeader[6];
for(i=0; i<6; i++) packetHeader[i] = _conn_read16();
if ((packetHeader[1] & 0x8000) != 0) { // Read answers
#ifdef MDNS_DEBUG_RX
Serial.printf("Reading answers RX: REQ, ID:%u, Q:%u, A:%u, NS:%u, ADD:%u\n", packetHeader[0], packetHeader[2], packetHeader[3], packetHeader[4], packetHeader[5]);
#endif
if (!_waitingForAnswers) {
#ifdef MDNS_DEBUG_RX
Serial.println("Not expecting any answers right now, returning");
#endif
_conn->flush();
return;
}
int numAnswers = packetHeader[3];
// Assume that the PTR answer always comes first and that it is always accompanied by a TXT, SRV and A answer in the same packet.
if (numAnswers != 4) {
#ifdef MDNS_DEBUG_RX
Serial.println("Expected a packet with 4 answers, returning");
#endif
_conn->flush();
return;
}
uint8_t tmp8;
uint16_t answerPort = 0;
uint8_t answerIp[4] = { 0,0,0,0 };
char answerHostName[255];
bool serviceMatch = false;
MDNSAnswer *answer;
uint8_t partsCollected = 0;
// Clear answer list
if (_newQuery) {
int numAnswers = _getNumAnswers();
for (int n = numAnswers - 1; n >= 0; n--) {
answer = _getAnswerFromIdx(n);
os_free(answer->hostname);
os_free(answer);
answer = 0;
}
_answers = 0;
_newQuery = false;
}
while (numAnswers--) {
// Read name
do {
tmp8 = _conn_read8();
if (tmp8 & 0xC0) { // Compressed pointer (not supported)
tmp8 = _conn_read8();
break;
}
if (tmp8 == 0x00) { // Énd of name
break;
}
_conn_readS(serviceName, tmp8);
serviceName[tmp8] = '\0';
#ifdef MDNS_DEBUG_RX
Serial.printf(" %d ", tmp8);
for (int n = 0; n < tmp8; n++) {
Serial.printf("%02x ", serviceName[n]);
}
Serial.println();
#endif
if (serviceName[0] == '_') {
if (strcmp(&serviceName[1], _query->_service) == 0) {
serviceMatch = true;
#ifdef MDNS_DEBUG_RX
Serial.printf("found matching service: %s\n", _query->_service);
#endif
}
}
} while (true);
uint16_t answerType = _conn_read16(); // Read type
uint16_t answerClass = _conn_read16(); // Read class
uint32_t answerTtl = _conn_read32(); // Read ttl
uint16_t answerRdlength = _conn_read16(); // Read rdlength
#ifdef MDNS_DEBUG_RX
Serial.printf("type: %04x rdlength: %d\n", answerType, answerRdlength);
#endif
if (answerType == MDNS_TYPE_PTR) {
partsCollected |= 0x01;
_conn_readS(hostName, answerRdlength); // Read rdata
#ifdef MDNS_DEBUG_RX
for (int n = 0; n < answerRdlength; n++) {
Serial.printf("%02x ", hostName[n]);
}
Serial.println();
#endif
}
if (answerType == MDNS_TYPE_TXT) {
partsCollected |= 0x02;
_conn_readS(hostName, answerRdlength); // Read rdata
#ifdef MDNS_DEBUG_RX
for (int n = 0; n < answerRdlength; n++) {
Serial.printf("%02x ", hostName[n]);
}
Serial.println();
#endif
}
if (answerType == MDNS_TYPE_SRV) {
partsCollected |= 0x04;
uint16_t answerPrio = _conn_read16(); // Read priority
uint16_t answerWeight = _conn_read16(); // Read weight
answerPort = _conn_read16(); // Read port
// Read hostname
tmp8 = _conn_read8();
if (tmp8 & 0xC0) { // Compressed pointer (not supported)
Serial.println("Skipping compressed pointer");
tmp8 = _conn_read8();
}
else {
_conn_readS(answerHostName, tmp8);
answerHostName[tmp8] = '\0';
#ifdef MDNS_DEBUG_RX
Serial.printf(" %d ", tmp8);
for (int n = 0; n < tmp8; n++) {
Serial.printf("%02x ", answerHostName[n]);
}
Serial.printf("\n%s\n", answerHostName);
#endif
if (answerRdlength - (6 + 1 + tmp8) > 0) { // Skip any remaining rdata
_conn_readS(hostName, answerRdlength - (6 + 1 + tmp8));
}
}
}
if (answerType == MDNS_TYPE_A) {
partsCollected |= 0x08;
for (int i = 0; i < 4; i++) {
answerIp[i] = _conn_read8();
}
}
if ((partsCollected == 0x0F) && serviceMatch) {
#ifdef MDNS_DEBUG_RX
Serial.println("All answers parsed, adding to _answers list..");
#endif
// Add new answer to answer list
if (_answers == 0) {
_answers = (struct MDNSAnswer*)(os_malloc(sizeof(struct MDNSAnswer)));
answer = _answers;
}
else {
answer = _answers;
while (answer->next != 0) {
answer = _answers->next;
}
answer->next = (struct MDNSAnswer*)(os_malloc(sizeof(struct MDNSAnswer)));
answer = answer->next;
}
answer->next = 0;
answer->hostname = 0;
// Populate new answer
answer->port = answerPort;
for (int i = 0; i < 4; i++) {
answer->ip[i] = answerIp[i];
}
answer->hostname = (char *)os_malloc(strlen(answerHostName) + 1);
os_strcpy(answer->hostname, answerHostName);
}
}
_conn->flush();
return;
}
// PARSE REQUEST NAME
hostNameLen = _conn_read8();
_conn_readS(hostName, hostNameLen);
hostName[hostNameLen] = '\0';
if(hostName[0] == '_'){
serviceParsed = true;
memcpy(serviceName, hostName+1, hostNameLen);
serviceNameLen = hostNameLen-1;
hostNameLen = 0;
}
if(hostNameLen > 0 && !_hostName.equals(hostName) && !_instanceName.equals(hostName)){
#ifdef MDNS_DEBUG_ERR
Serial.printf("ERR_NO_HOST: %s\n", hostName);
Serial.printf("hostname: %s\n", _hostName.c_str() );
Serial.printf("instance: %s\n", _instanceName.c_str() );
#endif
_conn->flush();
return;
}
if(!serviceParsed){
serviceNameLen = _conn_read8();
_conn_readS(serviceName, serviceNameLen);
serviceName[serviceNameLen] = '\0';
if(serviceName[0] == '_'){
memmove(serviceName, serviceName+1, serviceNameLen);
serviceNameLen--;
serviceParsed = true;
} else if(serviceNameLen == 5 && strcmp("local", serviceName) == 0){
tmp = _conn_read8();
if(tmp == 0){
serviceParsed = true;
serviceNameLen = 0;
protoParsed = true;
protoNameLen = 0;
localParsed = true;
} else {
#ifdef MDNS_DEBUG_ERR
Serial.printf("ERR_FQDN: %s\n", serviceName);
#endif
_conn->flush();
return;
}
} else {
#ifdef MDNS_DEBUG_ERR
Serial.printf("ERR_SERVICE: %s\n", serviceName);
#endif
_conn->flush();
return;
}
}
if(!protoParsed){
protoNameLen = _conn_read8();
_conn_readS(protoName, protoNameLen);
protoName[protoNameLen] = '\0';
if(protoNameLen == 4 && protoName[0] == '_'){
memmove(protoName, protoName+1, protoNameLen);
protoNameLen--;
protoParsed = true;
} else if(strcmp("services", serviceName) == 0 && strcmp("_dns-sd", protoName) == 0){
_conn->flush();
advertiseServices();
return;
} else {
#ifdef MDNS_DEBUG_ERR
Serial.printf("ERR_PROTO: %s\n", protoName);
#endif
_conn->flush();
return;
}
}
if(!localParsed){
char localName[32];
uint8_t localNameLen = _conn_read8();
_conn_readS(localName, localNameLen);
localName[localNameLen] = '\0';
tmp = _conn_read8();
if(localNameLen == 5 && strcmp("local", localName) == 0 && tmp == 0){
localParsed = true;
} else {
#ifdef MDNS_DEBUG_ERR
Serial.printf("ERR_FQDN: %s\n", localName);
#endif
_conn->flush();
return;
}
}
if(serviceNameLen > 0 && protoNameLen > 0){
servicePort = _getServicePort(serviceName, protoName);
if(servicePort == 0){
#ifdef MDNS_DEBUG_ERR
Serial.printf("ERR_NO_SERVICE: %s\n", serviceName);
#endif
_conn->flush();
return;
}
} else if(serviceNameLen > 0 || protoNameLen > 0){
#ifdef MDNS_DEBUG_ERR
Serial.printf("ERR_SERVICE_PROTO: %s\n", serviceName);
#endif
_conn->flush();
return;
}
// RESPOND
#ifdef MDNS_DEBUG_RX
Serial.printf("RX: REQ, ID:%u, Q:%u, A:%u, NS:%u, ADD:%u\n", packetHeader[0], packetHeader[2], packetHeader[3], packetHeader[4], packetHeader[5]);
#endif
uint16_t currentType;
uint16_t currentClass;
int numQuestions = packetHeader[2];
if(numQuestions > 4) numQuestions = 4;
uint16_t questions[4];
int question = 0;
while(numQuestions--){
currentType = _conn_read16();
if(currentType & MDNS_NAME_REF){ //new header handle it better!
currentType = _conn_read16();
}
currentClass = _conn_read16();
if(currentClass & MDNS_CLASS_IN) questions[question++] = currentType;
if(numQuestions > 0){
if(_conn_read16() != 0xC00C){//new question but for another host/service
_conn->flush();
numQuestions = 0;
}
}
#ifdef MDNS_DEBUG_RX
Serial.printf("REQ: ");
if(hostNameLen > 0) Serial.printf("%s.", hostName);
if(serviceNameLen > 0) Serial.printf("_%s.", serviceName);
if(protoNameLen > 0) Serial.printf("_%s.", protoName);
Serial.printf("local. ");
if(currentType == MDNS_TYPE_AAAA) Serial.printf(" AAAA ");
else if(currentType == MDNS_TYPE_A) Serial.printf(" A ");
else if(currentType == MDNS_TYPE_PTR) Serial.printf(" PTR ");
else if(currentType == MDNS_TYPE_SRV) Serial.printf(" SRV ");
else if(currentType == MDNS_TYPE_TXT) Serial.printf(" TXT ");
else Serial.printf(" 0x%04X ", currentType);
if(currentClass == MDNS_CLASS_IN) Serial.printf(" IN ");
else if(currentClass == MDNS_CLASS_IN_FLUSH_CACHE) Serial.printf(" IN[F] ");
else Serial.printf(" 0x%04X ", currentClass);
Serial.printf("\n");
#endif
}
uint8_t responseMask = 0;
for(i=0;i<question;i++){
if(questions[i] == MDNS_TYPE_A) responseMask |= 0x1;
else if(questions[i] == MDNS_TYPE_SRV) responseMask |= 0x3;
else if(questions[i] == MDNS_TYPE_TXT) responseMask |= 0x4;
else if(questions[i] == MDNS_TYPE_PTR) responseMask |= 0xF;
}
return _reply(responseMask, serviceName, protoName, servicePort);
}
INT32 _mongoSession::run()
{
INT32 rc = SDB_OK ;
BOOLEAN bigEndian = FALSE ;
UINT32 msgSize = 0 ;
UINT32 headerLen = sizeof( mongoMsgHeader ) - sizeof( INT32 ) ;
CHAR *pBuff = NULL ;
const CHAR *pBody = NULL ;
INT32 bodyLen = 0 ;
engine::pmdEDUMgr *pmdEDUMgr = NULL ;
std::vector< msgBuffer * >::iterator itr ;
if ( !_pEDUCB )
{
rc = SDB_SYS ;
goto error ;
}
pmdEDUMgr = _pEDUCB->getEDUMgr() ;
bigEndian = checkBigEndian() ;
while ( !_pEDUCB->isDisconnected() && !_socket.isClosed() )
{
_pEDUCB->resetInterrupt() ;
_pEDUCB->resetInfo( engine::EDU_INFO_ERROR ) ;
_pEDUCB->resetLsn() ;
rc = recvData( (CHAR*)&msgSize, sizeof(UINT32) ) ;
if ( rc )
{
if ( SDB_APP_FORCED != rc )
{
PD_LOG( PDERROR, "Session[%s] failed to recv msg size, "
"rc: %d", sessionName(), rc ) ;
}
break ;
}
if ( bigEndian )
{
}
if ( msgSize < headerLen || msgSize > SDB_MAX_MSG_LENGTH )
{
PD_LOG( PDERROR, "Session[%s] recv msg size[%d] is less than "
"mongoMsgHeader size[%d] or more than max msg size[%d]",
sessionName(), msgSize, sizeof( mongoMsgHeader ),
SDB_MAX_MSG_LENGTH ) ;
rc = SDB_INVALIDARG ;
break ;
}
else
{
pBuff = getBuff( msgSize + 1 ) ;
if ( !pBuff )
{
rc = SDB_OOM ;
break ;
}
*(UINT32*)pBuff = msgSize ;
rc = recvData( pBuff + sizeof(UINT32), msgSize - sizeof(UINT32) ) ;
if ( rc )
{
if ( SDB_APP_FORCED != rc )
{
PD_LOG( PDERROR, "Session failed to recv rest msg, rc: %d",
sessionName(), rc ) ;
}
break ;
}
pBuff[ msgSize ] = 0 ;
{
resetBuffers() ;
_converter->loadFrom( pBuff, msgSize ) ;
rc = _converter->convert( _inBufferVec ) ;
if ( SDB_OK != rc )
{
if ( SDB_OPTION_NOT_SUPPORT == rc )
{
}
else
{
goto error ;
}
}
if ( _preProcessMsg( _converter->getParser(),
_resource, _contextBuff ) )
{
goto reply ;
}
itr = _inBufferVec.begin() ;
for ( ; itr != _inBufferVec.end() ; ++itr )
{
_pEDUCB->incEventCount() ;
_needReply = FALSE ;
if ( SDB_OK != ( rc = pmdEDUMgr->activateEDU( _pEDUCB ) ) )
{
PD_LOG( PDERROR, "Session[%s] activate edu failed, rc: %d",
sessionName(), rc ) ;
goto error ;
}
rc = _processMsg( (*itr)->data() ) ;
if ( rc )
{
if ( SDB_DMS_CS_EXIST == rc &&
OP_CMD_CREATE == _converter->getOpType())
{
_contextBuff.release() ;
}
else
{
goto reply ;
}
}
if ( SDB_OK != ( rc = pmdEDUMgr->waitEDU( _pEDUCB ) ) )
{
PD_LOG( PDERROR, "Session[%s] wait edu failed, rc: %d",
sessionName(), rc ) ;
goto error ;
}
}
reply:
pBody = _contextBuff.data() ;
bodyLen = _contextBuff.size() ;
INT32 rcTmp = _reply( &_replyHeader, pBody, bodyLen ) ;
if ( rcTmp )
{
PD_LOG( PDERROR, "Session[%s] failed to send response, rc: %d",
sessionName(), rcTmp ) ;
goto error ;
}
pBody = NULL ;
bodyLen = 0 ;
}
}
} // end while
done:
disconnect() ;
return rc ;
error:
goto done ;
}
void MavlinkFTP::send(const hrt_abstime t)
{
if (_work_buffer1 || _work_buffer2) {
// free the work buffers if they are not used for a while
if (hrt_elapsed_time(&_last_work_buffer_access) > 2000000) {
if (_work_buffer1) {
delete[] _work_buffer1;
_work_buffer1 = nullptr;
}
if (_work_buffer2) {
delete[] _work_buffer2;
_work_buffer2 = nullptr;
}
}
}
// Anything to stream?
if (!_session_info.stream_download) {
return;
}
#ifndef MAVLINK_FTP_UNIT_TEST
// Skip send if not enough room
unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
#ifdef MAVLINK_FTP_DEBUG
warnx("MavlinkFTP::send max_bytes_to_send(%d) get_free_tx_buf(%d)", max_bytes_to_send, _mavlink->get_free_tx_buf());
#endif
if (max_bytes_to_send < get_size()) {
return;
}
#endif
// Send stream packets until buffer is full
bool more_data;
do {
more_data = false;
ErrorCode error_code = kErrNone;
mavlink_file_transfer_protocol_t ftp_msg;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_msg.payload[0]);
payload->seq_number = _session_info.stream_seq_number;
payload->session = 0;
payload->opcode = kRspAck;
payload->req_opcode = kCmdBurstReadFile;
payload->offset = _session_info.stream_offset;
_session_info.stream_seq_number++;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream send: offset %d", _session_info.stream_offset);
#endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (_session_info.stream_offset >= _session_info.file_size) {
error_code = kErrEOF;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: sending Nak EOF");
#endif
}
if (error_code == kErrNone) {
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: seek fail");
#endif
}
}
if (error_code == kErrNone) {
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
error_code = kErrFailErrno;
#ifdef MAVLINK_FTP_DEBUG
warnx("stream download: read fail");
#endif
} else {
payload->size = bytes_read;
_session_info.stream_offset += bytes_read;
_session_info.stream_chunk_transmitted += bytes_read;
}
}
if (error_code != kErrNone) {
payload->opcode = kRspNak;
payload->size = 1;
uint8_t *pData = &payload->data[0];
*pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error
if (error_code == kErrFailErrno) {
int r_errno = errno;
payload->size = 2;
payload->data[1] = r_errno;
}
_session_info.stream_download = false;
} else {
#ifndef MAVLINK_FTP_UNIT_TEST
if (max_bytes_to_send < (get_size() * 2)) {
more_data = false;
/* perform transfers in 35K chunks - this is determined empirical */
if (_session_info.stream_chunk_transmitted > 35000) {
payload->burst_complete = true;
_session_info.stream_download = false;
_session_info.stream_chunk_transmitted = 0;
}
} else {
#endif
more_data = true;
payload->burst_complete = false;
#ifndef MAVLINK_FTP_UNIT_TEST
max_bytes_to_send -= get_size();
}
#endif
}
ftp_msg.target_system = _session_info.stream_target_system_id;
_reply(&ftp_msg);
} while (more_data);
}
/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id)
{
bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
ErrorCode errorCode = kErrNone;
if (!_ensure_buffers_exist()) {
PX4_ERR("Failed to allocate buffers");
errorCode = kErrFailErrno;
errno = ENOMEM;
goto out;
}
// basic sanity checks; must validate length before use
if (payload->size > kMaxDataLength) {
errorCode = kErrInvalidDataSize;
goto out;
}
// check the sequence number: if this is a resent request, resend the last response
if (_last_reply_valid) {
mavlink_file_transfer_protocol_t *last_reply = reinterpret_cast<mavlink_file_transfer_protocol_t *>(_last_reply);
PayloadHeader *last_payload = reinterpret_cast<PayloadHeader *>(&last_reply->payload[0]);
if (payload->seq_number + 1 == last_payload->seq_number) {
// this is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS
// resent the request
mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), last_reply);
return;
}
}
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size,
payload->offset);
#endif
switch (payload->opcode) {
case kCmdNone:
break;
case kCmdTerminateSession:
errorCode = _workTerminate(payload);
break;
case kCmdResetSessions:
errorCode = _workReset(payload);
break;
case kCmdListDirectory:
errorCode = _workList(payload);
break;
case kCmdOpenFileRO:
errorCode = _workOpen(payload, O_RDONLY);
break;
case kCmdCreateFile:
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
break;
case kCmdOpenFileWO:
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
break;
case kCmdReadFile:
errorCode = _workRead(payload);
break;
case kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id);
stream_send = true;
break;
case kCmdWriteFile:
errorCode = _workWrite(payload);
break;
case kCmdRemoveFile:
errorCode = _workRemoveFile(payload);
break;
case kCmdRename:
errorCode = _workRename(payload);
break;
case kCmdTruncateFile:
errorCode = _workTruncateFile(payload);
break;
case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload);
break;
case kCmdRemoveDirectory:
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
default:
errorCode = kErrUnknownCommand;
break;
}
out:
payload->seq_number++;
// handle success vs. error
if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode;
payload->opcode = kRspAck;
} else {
int r_errno = errno;
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
payload->size = 1;
if (r_errno == EEXIST) {
errorCode = kErrFailFileExists;
}
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = r_errno;
}
}
_last_reply_valid = false;
// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nack.
if (!stream_send || errorCode != kErrNone) {
// respond to the request
ftp_req->target_system = target_system_id;
_reply(ftp_req);
}
}
/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id)
{
bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
ErrorCode errorCode = kErrNone;
// basic sanity checks; must validate length before use
if (payload->size > kMaxDataLength) {
errorCode = kErrInvalidDataSize;
goto out;
}
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size, payload->offset);
#endif
switch (payload->opcode) {
case kCmdNone:
break;
case kCmdTerminateSession:
errorCode = _workTerminate(payload);
break;
case kCmdResetSessions:
errorCode = _workReset(payload);
break;
case kCmdListDirectory:
errorCode = _workList(payload);
break;
case kCmdOpenFileRO:
errorCode = _workOpen(payload, O_RDONLY);
break;
case kCmdCreateFile:
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
break;
case kCmdOpenFileWO:
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
break;
case kCmdReadFile:
errorCode = _workRead(payload);
break;
case kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id);
stream_send = true;
break;
case kCmdWriteFile:
errorCode = _workWrite(payload);
break;
case kCmdRemoveFile:
errorCode = _workRemoveFile(payload);
break;
case kCmdRename:
errorCode = _workRename(payload);
break;
case kCmdTruncateFile:
errorCode = _workTruncateFile(payload);
break;
case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload);
break;
case kCmdRemoveDirectory:
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
default:
errorCode = kErrUnknownCommand;
break;
}
out:
payload->seq_number++;
// handle success vs. error
if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode;
payload->opcode = kRspAck;
} else {
int r_errno = errno;
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
payload->size = 1;
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = r_errno;
}
}
// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nak.
if (!stream_send || errorCode != kErrNone) {
// respond to the request
ftp_req->target_system = target_system_id;
_reply(ftp_req);
}
}
void MDNSResponder::_parsePacket(){
int i;
char tmp;
bool serviceParsed = false;
bool protoParsed = false;
bool localParsed = false;
char hostName[255];
uint8_t hostNameLen;
char serviceName[32];
uint8_t serviceNameLen;
uint16_t servicePort;
char protoName[32];
uint8_t protoNameLen;
uint16_t packetHeader[6];
for(i=0; i<6; i++) packetHeader[i] = _conn_read16();
if((packetHeader[1] & 0x8000) != 0){ //not parsing responses yet
_conn->flush();
return;
}
// PARSE REQUEST NAME
hostNameLen = _conn_read8();
_conn_readS(hostName, hostNameLen);
hostName[hostNameLen] = '\0';
if(hostName[0] == '_'){
serviceParsed = true;
memcpy(serviceName, hostName+1, hostNameLen);
serviceNameLen = hostNameLen-1;
hostNameLen = 0;
}
if(hostNameLen > 0 && strcmp(_hostName, hostName) != 0){
#ifdef MDNS_DEBUG_ERR
os_printf("ERR_NO_HOST: %s\n", hostName);
#endif
_conn->flush();
return;
}
if(!serviceParsed){
serviceNameLen = _conn_read8();
_conn_readS(serviceName, serviceNameLen);
serviceName[serviceNameLen] = '\0';
if(serviceName[0] == '_'){
memcpy(serviceName, serviceName+1, serviceNameLen);
serviceNameLen--;
serviceParsed = true;
} else if(serviceNameLen == 5 && strcmp("local", serviceName) == 0){
tmp = _conn_read8();
if(tmp == 0){
serviceParsed = true;
serviceNameLen = 0;
protoParsed = true;
protoNameLen = 0;
localParsed = true;
} else {
#ifdef MDNS_DEBUG_ERR
os_printf("ERR_FQDN: %s\n", serviceName);
#endif
_conn->flush();
return;
}
} else {
#ifdef MDNS_DEBUG_ERR
os_printf("ERR_SERVICE: %s\n", serviceName);
#endif
_conn->flush();
return;
}
}
if(!protoParsed){
protoNameLen = _conn_read8();
_conn_readS(protoName, protoNameLen);
protoName[protoNameLen] = '\0';
if(protoNameLen == 4 && protoName[0] == '_'){
memcpy(protoName, protoName+1, protoNameLen);
protoNameLen--;
protoParsed = true;
} else {
#ifdef MDNS_DEBUG_ERR
os_printf("ERR_PROTO: %s\n", protoName);
#endif
_conn->flush();
return;
}
}
if(!localParsed){
char localName[32];
uint8_t localNameLen = _conn_read8();
_conn_readS(localName, localNameLen);
localName[localNameLen] = '\0';
tmp = _conn_read8();
if(localNameLen == 5 && strcmp("local", localName) == 0 && tmp == 0){
localParsed = true;
} else {
#ifdef MDNS_DEBUG_ERR
os_printf("ERR_FQDN: %s\n", localName);
#endif
_conn->flush();
return;
}
}
if(serviceNameLen > 0 && protoNameLen > 0){
servicePort = _getServicePort(serviceName, protoName);
if(servicePort == 0){
#ifdef MDNS_DEBUG_ERR
os_printf("ERR_NO_SERVICE: %s\n", serviceName);
#endif
_conn->flush();
return;
}
} else if(serviceNameLen > 0 || protoNameLen > 0){
#ifdef MDNS_DEBUG_ERR
os_printf("ERR_SERVICE_PROTO: %s\n", serviceName);
#endif
_conn->flush();
return;
}
// RESPOND
#ifdef MDNS_DEBUG_RX
os_printf("RX: REQ, ID:%u, Q:%u, A:%u, NS:%u, ADD:%u\n", packetHeader[0], packetHeader[2], packetHeader[3], packetHeader[4], packetHeader[5]);
#endif
uint16_t currentType;
uint16_t currentClass;
int numQuestions = packetHeader[2];
if(numQuestions > 4) numQuestions = 4;
uint16_t questions[4];
int question = 0;
while(numQuestions--){
currentType = _conn_read16();
if(currentType & MDNS_NAME_REF){ //new header handle it better!
currentType = _conn_read16();
}
currentClass = _conn_read16();
if(currentClass & MDNS_CLASS_IN) questions[question++] = currentType;
if(numQuestions > 0){
if(_conn_read16() != 0xC00C){//new question but for another host/service
_conn->flush();
numQuestions = 0;
}
}
#ifdef MDNS_DEBUG_RX
os_printf("REQ: ");
if(hostNameLen > 0) os_printf("%s.", hostName);
if(serviceNameLen > 0) os_printf("_%s.", serviceName);
if(protoNameLen > 0) os_printf("_%s.", protoName);
os_printf("local. ");
if(currentType == MDNS_TYPE_AAAA) os_printf(" AAAA ");
else if(currentType == MDNS_TYPE_A) os_printf(" A ");
else if(currentType == MDNS_TYPE_PTR) os_printf(" PTR ");
else if(currentType == MDNS_TYPE_SRV) os_printf(" SRV ");
else if(currentType == MDNS_TYPE_TXT) os_printf(" TXT ");
else os_printf(" 0x%04X ", currentType);
if(currentClass == MDNS_CLASS_IN) os_printf(" IN ");
else if(currentClass == MDNS_CLASS_IN_FLUSH_CACHE) os_printf(" IN[F] ");
else os_printf(" 0x%04X ", currentClass);
os_printf("\n");
#endif
}
uint8_t responseMask = 0;
for(i=0;i<question;i++){
if(questions[i] == MDNS_TYPE_A) responseMask |= 0x1;
else if(questions[i] == MDNS_TYPE_SRV) responseMask |= 0x3;
else if(questions[i] == MDNS_TYPE_TXT) responseMask |= 0x4;
else if(questions[i] == MDNS_TYPE_PTR) responseMask |= 0xF;
}
return _reply(responseMask, (serviceName), (protoName), servicePort);
}
