/***************************************************************************** * Copyright (c) 2008, University of Florida * All rights reserved. * * This file is part of OpenJAUS. OpenJAUS is distributed under the BSD * license. See the LICENSE file for details. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the University of Florida nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ****************************************************************************/ // File Name: replaceMessagesMessage.c // // Written By: Danny Kent (jaus AT dannykent DOT com), Tom Galluzzo (galluzzo // AT gmail DOT com) // // Version: 3.3.0a // // Date: 08/07/08 // // Description: This file defines the functionality of a ReplaceMessagesMessage // // Modified by: David M. Plasil (MountainTop Technology, Inc) to add Planner // messages to OpenJAUS: 6/13/08 #include #include "jaus.h" static const int commandCode = JAUS_REPLACE_MESSAGES; static const int maxDataSizeBytes = 0; static JausBoolean headerFromBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes); static JausBoolean headerToBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes); static JausBoolean dataFromBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes); static int dataToBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes); static void dataInitialize(ReplaceMessagesMessage message); static void dataDestroy(ReplaceMessagesMessage message); static unsigned int dataSize(ReplaceMessagesMessage message); // ************************************************************************* // // USER CONFIGURED FUNCTIONS // ************************************************************************* // // Initializes the message-specific fields static void dataInitialize(ReplaceMessagesMessage message) { // Set initial values of static message fields 1, 2, and 3 message->missionId = newJausUnsignedShort(0); message->taskId = newJausUnsignedShort(0); message->numMsgsToRemove = newJausUnsignedShort(0); // Set initial value for dynamic data field 3+n for unique message IDs message->uid = NULL; // Setup dynamic data fields [1+n+3m to 3+n+3m] for task messages. message->command = jausArrayCreate(); } // Destructs the message-specific fields static void dataDestroy(ReplaceMessagesMessage message) { // Free dynamically allocated data field 3+n if(message->uid != NULL) { free(message->uid); message->uid = NULL; } // Free dynamically allocated array of data fields [1+n+3m to 3+n+3m] jausArrayDestroy(message->command,(void *)missionCommandDestroy); } //UNPACK the external component's buffered data into a JAUS message data area to //complete receive and then return boolean of success. static JausBoolean dataFromBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes) { int index = 0; int i; JausMissionCommand tempObject; //used to unpack fields [1+n+3m to 3+n+3m] JausUnsignedShort tempNumMsgsInsert = 0;//used to unpack field 4+n if(bufferSizeBytes == message->dataSize) { // Unpack data fields 1, 2, and 3 from Buffer if(!jausUnsignedShortFromBuffer(&message->missionId, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; if(!jausUnsignedShortFromBuffer(&message->taskId, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; if(!jausUnsignedShortFromBuffer(&message->numMsgsToRemove, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; //Dynamically allocate memory for array of unique message //IDs in Task and then unpack data field 3+n from Buffer. message->uid = (JausUnsignedShort *)malloc (message->numMsgsToRemove * JAUS_UNSIGNED_SHORT_SIZE_BYTES); for(i = 0; i < message->numMsgsToRemove; i++) { if(!jausUnsignedShortFromBuffer(&message->uid[i],buffer+index,bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; } // Unpack data field 4+n from Buffer if(!jausUnsignedShortFromBuffer(&tempNumMsgsInsert, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; //Setup to unpack data fields [1+n+3m to 3+n+3m] based on field 4+n //into the JausArray. For each mission message specified to be inserted //dynamically allocate a JausMissionCommand struct (initialized), unpack //fields [1+n+3m to 3+n+3m] from Buffer to the struct, and then update the //JausArray book keeping. for(i = 0; i < tempNumMsgsInsert; i++) { tempObject = NULL;//jausMissionCommandCreate(); if(!missionCommandFromBuffer(&tempObject,buffer+index,bufferSizeBytes-index)) return JAUS_FALSE; index += missionCommandSize(tempObject); //update JausArray mechanizm. jausArrayAdd(message->command, tempObject); } return JAUS_TRUE; } else { return JAUS_FALSE; } } //PACK the component's JAUS message data fields into a buffer to setup for //transmit to an external component and then return number of bytes put into //the buffer. static int dataToBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes) { int index = 0; int i; JausMissionCommand tempObject; //used to pack data fields 1+n+3m to 3+n+3m JausUnsignedShort tempShort; if(bufferSizeBytes >= dataSize(message)) { // Pack data fields 1, 2, and 3 to Buffer if(!jausUnsignedShortToBuffer(message->missionId, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; if(!jausUnsignedShortToBuffer(message->taskId, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; if(!jausUnsignedShortToBuffer(message->numMsgsToRemove, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; // Pack data field(s) 3+n based on field 3 to Buffer from dynamic array. for(i = 0; i < message->numMsgsToRemove; i++) { if(!jausUnsignedShortToBuffer(message->uid[i], buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; } if ( message->command->elementCount > JAUS_UNSIGNED_SHORT_MAX_VALUE) { //DMP DO: LOG Error. return JAUS_FALSE; } tempShort = message->command->elementCount; // Pack data field 4+n to Buffer if(!jausUnsignedShortToBuffer(tempShort,buffer+index,bufferSizeBytes-index)) return JAUS_FALSE; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; // Pack data fields [1+n+3m to 3+n+3m] based on field 4+n // to Buffer from dyanamic array. for(i = 0; i < message->command->elementCount; i++) { tempObject = (JausMissionCommand)(message->command->elementData[i]); if(!missionCommandToBuffer(tempObject, buffer+index, bufferSizeBytes-index)) return JAUS_FALSE; index += missionCommandSize(tempObject); } } return index; } // Returns number of bytes put into the buffer static unsigned int dataSize(ReplaceMessagesMessage message) { unsigned int index = 0; int i; //static data fields 1, 2, and 3 index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; //dynamic data field 3+n index += message->numMsgsToRemove * JAUS_UNSIGNED_SHORT_SIZE_BYTES; index += JAUS_UNSIGNED_SHORT_SIZE_BYTES; //dynamic data fields [1+n+3m to 3+n+3m] for ( i = 0; i < message->command->elementCount; i++) { index += missionCommandSize(message->command->elementData[i]); } return index; } // ************************************************************************* // // NON-USER CONFIGURED FUNCTIONS // ************************************************************************* // ReplaceMessagesMessage replaceMessagesMessageCreate(void) { ReplaceMessagesMessage message; message = (ReplaceMessagesMessage)malloc( sizeof(ReplaceMessageStruct) ); if(message == NULL) { return NULL; } // Initialize Values message->properties.priority = JAUS_DEFAULT_PRIORITY; message->properties.ackNak = JAUS_ACK_NAK_NOT_REQUIRED; message->properties.scFlag = JAUS_NOT_SERVICE_CONNECTION_MESSAGE; message->properties.expFlag = JAUS_NOT_EXPERIMENTAL_MESSAGE; message->properties.version = JAUS_VERSION_3_3; message->properties.reserved = 0; message->commandCode = commandCode; message->destination = jausAddressCreate(); message->source = jausAddressCreate(); message->dataFlag = JAUS_SINGLE_DATA_PACKET; message->dataSize = maxDataSizeBytes; message->sequenceNumber = 0; dataInitialize(message); message->dataSize = dataSize(message); return message; } void replaceMessagesMessageDestroy(ReplaceMessagesMessage message) { dataDestroy(message); jausAddressDestroy(message->source); jausAddressDestroy(message->destination); free(message); } JausBoolean replaceMessagesMessageFromBuffer(ReplaceMessagesMessage message,unsigned char* buffer,unsigned int bufferSizeBytes) { int index = 0; if(headerFromBuffer(message, buffer+index, bufferSizeBytes-index)) { index += JAUS_HEADER_SIZE_BYTES; if(dataFromBuffer(message, buffer+index, bufferSizeBytes-index)) { return JAUS_TRUE; } else { return JAUS_FALSE; } } else { return JAUS_FALSE; } } JausBoolean replaceMessagesMessageToBuffer(ReplaceMessagesMessage message, unsigned char *buffer,unsigned int bufferSizeBytes) { if(bufferSizeBytes < replaceMessagesMessageSize(message)) { return JAUS_FALSE; //improper size } else { message->dataSize = dataToBuffer(message, buffer+JAUS_HEADER_SIZE_BYTES, bufferSizeBytes - JAUS_HEADER_SIZE_BYTES); if(headerToBuffer(message, buffer, bufferSizeBytes)) { return JAUS_TRUE; } else { return JAUS_FALSE; // headerToReplaceBuffer failed } } } ReplaceMessagesMessage replaceMessagesMessageFromJausMessage(JausMessage jausMessage) { ReplaceMessagesMessage message; if(jausMessage->commandCode != commandCode) { return NULL; // Wrong message type } else { message = (ReplaceMessagesMessage)malloc( sizeof(ReplaceMessageStruct) ); if(message == NULL) { return NULL; } message->properties.priority = jausMessage->properties.priority; message->properties.ackNak = jausMessage->properties.ackNak; message->properties.scFlag = jausMessage->properties.scFlag; message->properties.expFlag = jausMessage->properties.expFlag; message->properties.version = jausMessage->properties.version; message->properties.reserved = jausMessage->properties.reserved; message->commandCode = jausMessage->commandCode; message->destination = jausAddressCreate(); *message->destination = *jausMessage->destination; message->source = jausAddressCreate(); *message->source = *jausMessage->source; message->dataSize = jausMessage->dataSize; message->dataFlag = jausMessage->dataFlag; message->sequenceNumber = jausMessage->sequenceNumber; dataInitialize(message); // Unpack jausMessage->data if(dataFromBuffer(message, jausMessage->data, jausMessage->dataSize)) { return message; } else { return NULL; } } } JausMessage replaceMessagesMessageToJausMessage(ReplaceMessagesMessage message) { JausMessage jausMessage; jausMessage = (JausMessage)malloc( sizeof(struct JausMessageStruct) ); if(jausMessage == NULL) { return NULL; } jausMessage->properties.priority = message->properties.priority; jausMessage->properties.ackNak = message->properties.ackNak; jausMessage->properties.scFlag = message->properties.scFlag; jausMessage->properties.expFlag = message->properties.expFlag; jausMessage->properties.version = message->properties.version; jausMessage->properties.reserved = message->properties.reserved; jausMessage->commandCode = message->commandCode; jausMessage->destination = jausAddressCreate(); *jausMessage->destination = *message->destination; jausMessage->source = jausAddressCreate(); *jausMessage->source = *message->source; jausMessage->dataSize = dataSize(message); jausMessage->dataFlag = message->dataFlag; jausMessage->sequenceNumber = message->sequenceNumber; jausMessage->data = (unsigned char *)malloc(jausMessage->dataSize); jausMessage->dataSize = dataToBuffer(message,jausMessage->data,jausMessage->dataSize); return jausMessage; } unsigned int replaceMessagesMessageSize(ReplaceMessagesMessage message) { return (unsigned int)(dataSize(message) + JAUS_HEADER_SIZE_BYTES); } //********************* PRIVATE HEADER FUNCTIONS **********************// static JausBoolean headerFromBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes) { if(bufferSizeBytes < JAUS_HEADER_SIZE_BYTES) { return JAUS_FALSE; } else { // unpack header message->properties.priority = (buffer[0] & 0x0F); message->properties.ackNak = ((buffer[0] >> 4) & 0x03); message->properties.scFlag = ((buffer[0] >> 6) & 0x01); message->properties.expFlag = ((buffer[0] >> 7) & 0x01); message->properties.version = (buffer[1] & 0x3F); message->properties.reserved = ((buffer[1] >> 6) & 0x03); message->commandCode = buffer[2] + (buffer[3] << 8); message->destination->instance = buffer[4]; message->destination->component = buffer[5]; message->destination->node = buffer[6]; message->destination->subsystem = buffer[7]; message->source->instance = buffer[8]; message->source->component = buffer[9]; message->source->node = buffer[10]; message->source->subsystem = buffer[11]; message->dataSize = buffer[12] + ((buffer[13] & 0x0F) << 8); message->dataFlag = ((buffer[13] >> 4) & 0x0F); message->sequenceNumber = buffer[14] + (buffer[15] << 8); return JAUS_TRUE; } } static JausBoolean headerToBuffer(ReplaceMessagesMessage message,unsigned char *buffer,unsigned int bufferSizeBytes) { JausUnsignedShort *propertiesPtr = (JausUnsignedShort*)&message->properties; if(bufferSizeBytes < JAUS_HEADER_SIZE_BYTES) { return JAUS_FALSE; } else { buffer[0] = (unsigned char)(*propertiesPtr & 0xFF); buffer[1] = (unsigned char)((*propertiesPtr & 0xFF00) >> 8); buffer[2] = (unsigned char)(message->commandCode & 0xFF); buffer[3] = (unsigned char)((message->commandCode & 0xFF00) >> 8); buffer[4] = (unsigned char)(message->destination->instance & 0xFF); buffer[5] = (unsigned char)(message->destination->component & 0xFF); buffer[6] = (unsigned char)(message->destination->node & 0xFF); buffer[7] = (unsigned char)(message->destination->subsystem & 0xFF); buffer[8] = (unsigned char)(message->source->instance & 0xFF); buffer[9] = (unsigned char)(message->source->component & 0xFF); buffer[10] = (unsigned char)(message->source->node & 0xFF); buffer[11] = (unsigned char)(message->source->subsystem & 0xFF); buffer[12] = (unsigned char)(message->dataSize & 0xFF); buffer[13] = (unsigned char)((message->dataFlag & 0xFF) << 4) | (unsigned char)((message->dataSize & 0x0F00) >> 8); buffer[14] = (unsigned char)(message->sequenceNumber & 0xFF); buffer[15] = (unsigned char)((message->sequenceNumber & 0xFF00) >> 8); return JAUS_TRUE; } }