OSCMessage.cpp

00001 /*
00002 #include <OSCMessage.h>
00003  mbedOSC.cpp 
00004 */                    
00005    
00006 #include "mbed.h"
00007 #include "stdarg.h"
00008 #include "OSCMessage.h"
00009 #include "OSCMatch.h"
00010 #include "OSCTiming.h"
00011 
00012  extern osctime_t zerotime;
00013 /*=============================================================================
00014     CONSTRUCTORS / DESTRUCTOR
00015 =============================================================================*/
00016 
00017 //constructor with address
00018 OSCMessage::OSCMessage(const char * _address){
00019     setupMessage();
00020     setAddress(_address);
00021 }
00022 
00023 //constructor with nothing
00024 //just a placeholder since the message is invalid
00025 OSCMessage::OSCMessage(){
00026     setupMessage();
00027     error = INVALID_OSC;
00028 }
00029 
00030 //variable length constructor
00031 //for example OSCMessage msg("/address", "isf", 1, "two", 3.0);
00032 /* TODO: make this work
00033 OSCMessage::OSCMessage(const char * _address, char * types, ... ){
00034     setupMessage(_address);
00035 }
00036 
00037 */
00038 //sets up a new message
00039 void OSCMessage::setupMessage(){
00040     address = NULL;
00041     //setup the attributes
00042     dataCount = 0;
00043     error = OSC_OK;
00044     //setup the space for data
00045     data = NULL;
00046     //setup for filling the message
00047     incomingBuffer = NULL;
00048     incomingBufferSize = 0;
00049     incomingBufferFree = 0;
00050     clearIncomingBuffer();
00051     //set the decode state
00052     decodeState = STANDBY;
00053 }
00054 
00055 //DESTRUCTOR
00056 OSCMessage::~OSCMessage(){
00057     //free everything that needs to be freed
00058     //free the address
00059     free(address);
00060     //free the data
00061     empty();
00062     //free the filling buffer
00063     free(incomingBuffer);
00064 }
00065 
00066 OSCMessage& OSCMessage::empty(){
00067     error = OSC_OK;
00068     //free each of hte data in the array
00069     for (int i = 0; i < dataCount; i++){
00070         OSCData * datum = getOSCData(i);
00071         //explicitly destruct the data
00072         //datum->~OSCData();
00073         delete datum;
00074     }
00075     //and free the array
00076     free(data);
00077     data = NULL;
00078     dataCount = 0;
00079     decodeState = STANDBY;
00080     clearIncomingBuffer();
00081     return *this;
00082 }
00083 
00084 //COPY
00085 OSCMessage::OSCMessage(OSCMessage * msg){
00086     //start with a message with the same address
00087     setupMessage();
00088     setAddress(msg->address);
00089     //add each of the data to the other message
00090     for (int i = 0; i < msg->dataCount; i++){
00091         add(msg->data[i]);
00092     }
00093 }
00094 void OSCMessage::copy(OSCMessage * msg){
00095     msg->setAddress(address);
00096     for (int i = 0; i < dataCount; i++){
00097             msg->add(data[i]);
00098 
00099         }
00100 
00101 }
00102 /*=============================================================================
00103     GETTING DATA
00104 =============================================================================*/
00105 
00106 OSCData * OSCMessage::getOSCData(int position){
00107     if (position < dataCount){
00108         OSCData * datum = data[position];
00109         return datum;
00110     } else {
00111         error = INDEX_OUT_OF_BOUNDS;
00112         return NULL;
00113     }
00114 }
00115 
00116 int32_t OSCMessage::getInt(int position){
00117     OSCData * datum = getOSCData(position);
00118     if (!hasError()){
00119         return datum->getInt();
00120     } else {
00121             return -1;
00122     }
00123 }
00124 
00125 osctime_t OSCMessage::getTime(int position){
00126     OSCData * datum = getOSCData(position);
00127     if (!hasError()){
00128         return datum->getTime();
00129     } else {
00130         return zerotime;
00131     }
00132 }
00133 
00134 float OSCMessage::getFloat(int position){
00135     OSCData * datum = getOSCData(position);
00136     if (!hasError()){
00137         return datum->getFloat();
00138     } else {
00139             return -1;
00140     }
00141 }
00142 
00143 double OSCMessage::getDouble(int position){
00144     OSCData * datum = getOSCData(position);
00145     if (!hasError()){
00146         return datum->getDouble();
00147     } else {
00148             return -1;
00149     }
00150 }
00151 
00152 bool  OSCMessage::getBoolean(int position){
00153     OSCData * datum = getOSCData(position);
00154     if (!hasError()){
00155         return datum->getBoolean();
00156     } else {
00157             return -1;
00158     }
00159 }
00160 
00161 
00162 int OSCMessage::getString(int position, char * buffer){
00163     OSCData * datum = getOSCData(position);
00164     if (!hasError()){
00165         return datum->getString(buffer, datum->bytes);
00166     } else {
00167 
00168             return -1;
00169 
00170     }
00171 }
00172 
00173 int OSCMessage::getString(int position, char * buffer, int bufferSize){
00174     OSCData * datum = getOSCData(position);
00175     if (!hasError()){
00176         //the number of bytes to copy is the smaller between the buffer size and the datum's byte length
00177         int copyBytes = bufferSize < datum->bytes? bufferSize : datum->bytes;
00178         return datum->getString(buffer, copyBytes);
00179     } else {
00180             return -1;
00181     }
00182 }
00183 
00184 int OSCMessage::getString(int position, char * buffer, int bufferSize, int offset, int size){
00185     OSCData * datum = getOSCData(position);
00186     if (!hasError()){
00187         //the number of bytes to copy is the smaller between the buffer size and the datum's byte length
00188         int copyBytes = bufferSize < datum->bytes? bufferSize : datum->bytes;
00189         return datum->getString(buffer, copyBytes, offset, size);
00190     } else {
00191             return -1;
00192     }
00193 }
00194 
00195 
00196 int OSCMessage::getBlob(int position, uint8_t * buffer){
00197     OSCData * datum = getOSCData(position);
00198     if (!hasError()){
00199         return datum->getBlob(buffer);
00200   } else {
00201         return -1;
00202   }
00203 }
00204 
00205 int OSCMessage::getBlob(int position, uint8_t * buffer, int bufferSize){
00206     OSCData * datum = getOSCData(position);
00207     if (!hasError()){
00208         return datum->getBlob(buffer, bufferSize);
00209   } else {
00210         return -1;
00211   }
00212 }
00213 
00214 int OSCMessage::getBlob(int position, uint8_t * buffer, int bufferSize, int offset, int size){
00215     OSCData * datum = getOSCData(position);
00216     if (!hasError()){
00217         return datum->getBlob(buffer, bufferSize, offset, size);
00218   } else {
00219         return -1;
00220   }
00221 }
00222 
00223 uint32_t OSCMessage::getBlobLength(int position)
00224 {
00225   OSCData * datum = getOSCData(position);
00226   if (!hasError()){
00227     return datum->getBlobLength();
00228   } else {
00229         return -1;
00230   }
00231 
00232 }
00233 
00234 char OSCMessage::getType(int position){
00235     OSCData * datum = getOSCData(position);
00236     if (!hasError()){
00237         return datum->type;
00238     } else {
00239             return '\0';
00240     }
00241 }
00242 
00243 int OSCMessage::getDataLength(int position){
00244     OSCData * datum = getOSCData(position);
00245     if (!hasError()){
00246         return datum->bytes;
00247     } else {
00248         return 0;
00249     }
00250 }
00251 
00252 /*=============================================================================
00253     TESTING DATA
00254 =============================================================================*/
00255 
00256 bool OSCMessage::testType(int position, char type){
00257     OSCData * datum = getOSCData(position);
00258     if (!hasError()){
00259         return datum->type == type;
00260     } else {
00261         return false;
00262     }
00263 }
00264 
00265 bool OSCMessage::isInt(int position){
00266     return testType(position, 'i');
00267 }
00268 
00269 bool OSCMessage::isTime(int position){
00270     return testType(position, 't');
00271 }
00272 
00273 
00274 bool OSCMessage::isFloat(int position){
00275     return testType(position, 'f');
00276 }
00277 
00278 bool OSCMessage::isBlob(int position){
00279     return testType(position, 'b');
00280 }
00281 
00282 bool OSCMessage::isChar(int position){
00283     return testType(position, 'c');
00284 }
00285 
00286 bool OSCMessage::isString(int position){
00287     return testType(position, 's');
00288 }
00289 
00290 bool OSCMessage::isDouble(int position){
00291     return testType(position, 'd');
00292 }
00293 bool OSCMessage::isBoolean(int position){
00294     return testType(position, 'T') || testType(position, 'F');
00295 }
00296 
00297 
00298 /*=============================================================================
00299     PATTERN MATCHING
00300 =============================================================================*/
00301 
00302 int OSCMessage::match(const  char * pattern, int addr_offset){
00303     int pattern_offset;
00304     int address_offset;
00305     int ret = osc_match(address + addr_offset, pattern, &pattern_offset, &address_offset);
00306     char * next = (char *) (address + addr_offset + pattern_offset);
00307     if (ret==3){
00308         return pattern_offset;
00309     } else if (pattern_offset > 0 && *next == '/'){
00310         return pattern_offset;
00311     } else {
00312         return 0;
00313     }
00314 }
00315 
00316 bool OSCMessage::fullMatch( const char * pattern, int addr_offset){
00317     int pattern_offset;
00318     int address_offset;
00319     int ret = osc_match(address + addr_offset, pattern, &address_offset, &pattern_offset);
00320     return (ret==3);
00321 }
00322 
00323 bool OSCMessage::dispatch(const char * pattern, void (*callback)(OSCMessage &), int addr_offset){
00324     if (fullMatch(pattern, addr_offset)){
00325         callback(*this);
00326         return true;
00327     } else {
00328         return false;
00329     }
00330 }
00331 
00332 bool OSCMessage::route(const char * pattern, void (*callback)(OSCMessage &, int), int initial_offset){
00333     int match_offset = match(pattern, initial_offset);
00334     if (match_offset>0){
00335         callback(*this, match_offset + initial_offset);
00336         return true;
00337     } else {
00338         return false;
00339     }
00340 }
00341 
00342 /*=============================================================================
00343     ADDRESS
00344  =============================================================================*/
00345 
00346 const char * OSCMessage::getAddress( int offset){
00347 
00348     return address;
00349 }
00350 
00351 int OSCMessage::getAddress(char * buffer, int offset, int len){
00352     strncpy(buffer, address+offset, len);
00353     return strlen(buffer);
00354 }
00355 
00356 OSCMessage& OSCMessage::setAddress(const char * _address){
00357     //free the previous address
00358     free(address); // are we sure address was allocated?
00359     //copy the address
00360     char * addressMemory = (char *) malloc( (strlen(_address) + 1) * sizeof(char) );
00361     if (addressMemory == NULL){
00362         error = ALLOCFAILED;
00363         address = NULL;
00364     } else {
00365         strcpy(addressMemory, _address);
00366         address = addressMemory;
00367     }
00368     return *this;
00369 }
00370 
00371 /*=============================================================================
00372     SIZE
00373 =============================================================================*/
00374 
00375 int OSCMessage::padSize(int _bytes){
00376     int space = (_bytes + 3) / 4;
00377     space *= 4;
00378     return space - _bytes;
00379 }
00380 
00381 //returns the number of OSCData in the OSCMessage
00382 int OSCMessage::size(){
00383     return dataCount;
00384 }
00385 
00386 int OSCMessage::bytes(){
00387     int messageSize = 0;
00388     //send the address
00389     int addrLen = strlen(address) + 1;
00390     messageSize += addrLen;
00391     //padding amount
00392     int addrPad = padSize(addrLen);
00393     messageSize += addrPad;
00394     //add the comma seperator
00395     messageSize += 1;
00396     //add the types
00397     messageSize += dataCount;
00398     //pad the types
00399     int typePad = padSize(dataCount + 1);   //for the comma
00400     if (typePad == 0){
00401          typePad = 4; // to make sure the type string is null terminated
00402     }
00403     messageSize+=typePad;
00404     //then the data
00405     for (int i = 0; i < dataCount; i++){
00406         OSCData * datum = getOSCData(i);
00407         messageSize+=datum->bytes;
00408         messageSize += padSize(datum->bytes);
00409     }
00410     return messageSize;
00411 }
00412 
00413 /*=============================================================================
00414     ERROR HANDLING
00415 =============================================================================*/
00416 
00417 bool OSCMessage::hasError(){
00418     bool retError = error != OSC_OK;
00419     //test each of the data
00420     for (int i = 0; i < dataCount; i++){
00421         OSCData * datum = getOSCData(i);
00422         retError |= datum->error != OSC_OK;
00423     }
00424     return retError;
00425 }
00426 
00427 OSCErrorCode OSCMessage::getError(){
00428     return error;
00429 }
00430 
00431 /*=============================================================================
00432     SENDING
00433  =============================================================================*/
00434 
00435 OSCMessage& OSCMessage::send(UDPSocket &p, const SocketAddress &address){
00436 
00437     char  buff[128];
00438     uint8_t lengthEnd;
00439     uint8_t lengthStart;
00440     uint8_t nullChar = '\0';
00441     buff[0]=nullChar;
00442 
00443     //don't send a message with errors
00444     if (hasError()){
00445         return *this;
00446     }
00447 
00448     //send the address
00449     int addrLen = strlen(address) + 1;
00450     //padding amount
00451     int addrPad = padSize(addrLen);
00452     //write it to the stream
00453     strcat(buff, address);
00454  //   p.write((uint8_t *) address, addrLen);
00455     //add the padding
00456     lengthStart=strlen(buff);
00457     lengthEnd=lengthStart+(4-(lengthStart%4));
00458 
00459     for(int i=lengthStart ; i<lengthEnd; i++){
00460         buff[i]=nullChar;
00461     }
00462 
00463 
00464 
00465     lengthStart=lengthEnd;
00466 
00467     //add the comma seperator
00468 
00469     buff[lengthEnd++]=',';
00470 
00471     /*
00472     p.write((uint8_t) ',');
00473     //add the types
00474     */
00475 
00476     for (int i = 0; i < dataCount; i++){
00477         buff[lengthEnd++]=(uint8_t) getType(i);
00478     //    p.write((uint8_t) getType(i));
00479     }
00480 
00481 
00482     //pad the types
00483     int typePad = padSize(dataCount + 1); // 1 is for the comma
00484     if (typePad == 0){
00485             typePad = 4;  // This is because the type string has to be null terminated
00486     }
00487 
00488 
00489     while(typePad--){
00490         buff[lengthEnd++]=nullChar;
00491     }
00492     //write the data
00493 
00494 
00495     for (int i = 0; i < dataCount; i++){
00496         OSCData * datum = getOSCData(i);
00497         if ((datum->type == 's') || (datum->type == 'b')){
00498             for(int i = 0; i < datum->bytes; i++){
00499                 buff[lengthEnd++]=datum->data.b[i];
00500             }
00501 
00502 
00503             int dataPad = padSize(datum->bytes);
00504             while(dataPad--){
00505                 buff[lengthEnd++]=nullChar;
00506             }
00507         } else  if (datum->type == 'd'){
00508             double d = BigEndian(datum->data.d); //TODO
00509             uint8_t * ptr = (uint8_t *) &d;
00510             for(int i = 0; i < 8; i++){
00511                 buff[lengthEnd++]=ptr[i];
00512             }
00513 
00514         }   else if (datum->type == 't'){
00515             osctime_t time =  datum->data.time;
00516             uint32_t d = BigEndian(time.seconds);
00517             uint8_t * ptr = (uint8_t *)    &d;
00518             for(int i = 0; i < 4; i++){
00519                 buff[lengthEnd++]=ptr[i];
00520             }
00521          //   p.write(ptr, 4);
00522             d = BigEndian(time.fractionofseconds);
00523             ptr = (uint8_t *)    &d;
00524             for(int i = 0; i < 4; i++){
00525                 buff[lengthEnd++]=ptr[i];
00526             }
00527           //  p.write(ptr, 4);
00528 
00529         }  else  if (datum->type == 'T' || datum->type == 'F')
00530                     { }
00531         else { // float or int
00532             uint32_t i = BigEndian(datum->data.i);
00533             uint8_t * ptr = (uint8_t *) &i;
00534             for (int i = 0; i < datum->bytes; i++){
00535                 buff[lengthEnd++]=ptr[i];
00536             }
00537       //      p.write(ptr, datum->bytes);
00538         }
00539     }
00540     p.sendto(address, buff, lengthEnd);
00541     return *this;
00542 }
00543 
00544 /*=============================================================================
00545     FILLING
00546  =============================================================================*/
00547 
00548 OSCMessage& OSCMessage::fill(uint8_t incomingByte){
00549     decode(incomingByte);
00550     return *this;
00551 }
00552 
00553 OSCMessage& OSCMessage::fill(uint8_t * incomingBytes, int length){
00554     while (length--){
00555         decode(*incomingBytes++);
00556     }
00557     return *this;
00558 }
00559 
00560 /*=============================================================================
00561     DECODING
00562  =============================================================================*/
00563 
00564 void OSCMessage::decodeAddress(){
00565     setAddress((char *) incomingBuffer);
00566     //change the error from invalide message
00567     error = OSC_OK;
00568     clearIncomingBuffer();
00569 }
00570 
00571 void OSCMessage::decodeType(uint8_t incomingByte){
00572     char type = incomingByte;
00573     add(type);
00574 }
00575 
00576 void OSCMessage::decodeData(uint8_t incomingByte){
00577     //get the first OSCData to re-set
00578     for (int i = 0; i < dataCount; i++){
00579         OSCData * datum = getOSCData(i);
00580         if (datum->error == INVALID_OSC){
00581             //set the contents of datum with the data received
00582             switch (datum->type){
00583                 case 'i':
00584                     if (incomingBufferSize == 4){
00585                         //parse the buffer as an int
00586                         union {
00587                             int32_t i;
00588                             uint8_t b[4];
00589                         } u;
00590                         memcpy(u.b, incomingBuffer, 4);
00591                         int32_t dataVal = BigEndian(u.i);
00592                         set(i, (int32_t)dataVal);
00593                         clearIncomingBuffer();
00594                     }
00595                     break;
00596                 case 'f':
00597                     if (incomingBufferSize == 4){
00598                         //parse the buffer as a float
00599                         union {
00600                             float f;
00601                             uint8_t b[4];
00602                         } u;
00603                         memcpy(u.b, incomingBuffer, 4);
00604                         float dataVal = BigEndian(u.f);
00605                         set(i, dataVal);
00606                         clearIncomingBuffer();
00607                     }
00608                     break;
00609                 case 'd':
00610                     if (incomingBufferSize == 8){
00611                         //parse the buffer as a double
00612                         union {
00613                             double d;
00614                             uint8_t b[8];
00615                         } u;
00616                         memcpy(u.b, incomingBuffer, 8);
00617                         double dataVal = BigEndian(u.d);
00618                         set(i, dataVal);
00619                         clearIncomingBuffer();
00620                     }
00621                     break;
00622          /*       case 't':
00623                     if (incomingBufferSize == 8){
00624                         //parse the buffer as a timetag
00625                         union {
00626                             osctime_t t;
00627                             uint8_t b[8];
00628                         } u;
00629                         memcpy(u.b, incomingBuffer, 8);
00630 
00631                         u.t.seconds = BigEndian(u.t.seconds);
00632                         u.t.fractionofseconds = BigEndian(u.t.fractionofseconds);
00633                         set(i, u.t);
00634                         clearIncomingBuffer();
00635                     }
00636                     break;
00637             */
00638                 case 's':
00639                     if (incomingByte == 0){
00640                         char * str = (char *) incomingBuffer;
00641                         set(i, str);
00642                         clearIncomingBuffer();
00643                         decodeState = DATA_PADDING;
00644                     }
00645                     break;
00646                 case 'b':
00647                     if (incomingBufferSize > 4){
00648                         //compute the expected blob size
00649                         union {
00650                             uint32_t i;
00651                             uint8_t b[4];
00652                         } u;
00653                         memcpy(u.b, incomingBuffer, 4);
00654                         uint32_t blobLength = BigEndian(u.i);
00655                         if (incomingBufferSize == (int)(blobLength + 4)){
00656                             set(i, incomingBuffer + 4, blobLength);
00657                             clearIncomingBuffer();
00658                             decodeState = DATA_PADDING;
00659                         }
00660 
00661                     }
00662                     break;
00663             }
00664             //break out of the for loop once we've selected the first invalid message
00665             break;
00666         }
00667     }
00668 }
00669 
00670 //does not validate the incoming OSC for correctness
00671 void OSCMessage::decode(uint8_t incomingByte){
00672     addToIncomingBuffer(incomingByte);
00673     switch (decodeState){
00674         case STANDBY:
00675             if (incomingByte == '/'){
00676                 decodeState = ADDRESS;
00677             }
00678             break;
00679         case ADDRESS:
00680             if (incomingByte == 0){
00681                 //end of the address
00682                 //decode the address
00683                 decodeAddress();
00684                 //next state
00685                 decodeState = ADDRESS_PADDING;
00686             }
00687             break;
00688         case ADDRESS_PADDING:
00689             //it does not count the padding
00690             if (incomingByte==','){
00691                 //next state
00692                 decodeState = TYPES;
00693                 clearIncomingBuffer();
00694             }
00695             break;
00696         case TYPES:
00697             if (incomingByte != 0){
00698                 //next state
00699                 decodeType(incomingByte);
00700             } else {
00701                 decodeState = TYPES_PADDING;
00702             }
00703             //FALL THROUGH to test if it should go to the data state
00704         case TYPES_PADDING: {
00705                 //compute the padding size for the types
00706                 //to determine the start of the data section
00707             int typePad = padSize(dataCount + 1); // 1 is the comma
00708                 if (typePad == 0){
00709                     typePad = 4;     // to make sure it will be null terminated
00710                 }
00711                 if (incomingBufferSize == (typePad + dataCount)){
00712                     clearIncomingBuffer();
00713                     decodeState = DATA;
00714                 }
00715             }
00716             break;
00717         case DATA:
00718             decodeData(incomingByte);
00719             break;
00720         case DATA_PADDING:{
00721                 //get the last valid data
00722                 for (int i = dataCount - 1; i >= 0; i--){
00723                     OSCData * datum = getOSCData(i);
00724                     if (datum->error == OSC_OK){
00725                         //compute the padding size for the data
00726                         int dataPad = padSize(datum->bytes);
00727                         //  if there is no padding required, switch back to DATA, and don't clear the incomingBuffer because it holds next data
00728                         if (dataPad == 0){
00729                              decodeState = DATA;
00730                         }
00731                         else if (incomingBufferSize == dataPad){
00732                             clearIncomingBuffer();
00733                             decodeState = DATA;
00734                         }
00735                         break;
00736                     }
00737                 }
00738             }
00739             break;
00740         case DONE:
00741             break;
00742     }
00743 }
00744 
00745 
00746 /*=============================================================================
00747     INCOMING BUFFER MANAGEMENT
00748  =============================================================================*/
00749 #define OSCPREALLOCATEIZE 16
00750 void OSCMessage::addToIncomingBuffer(uint8_t incomingByte){
00751     //realloc some space for the new byte and stick it on the end
00752     if(incomingBufferFree>0)
00753     {
00754             incomingBuffer[incomingBufferSize++] = incomingByte;
00755             incomingBufferFree--;
00756     }
00757     else
00758     {
00759 
00760         incomingBuffer = (uint8_t *) realloc ( incomingBuffer, incomingBufferSize + 1 + OSCPREALLOCATEIZE);
00761         if (incomingBuffer != NULL){
00762             incomingBuffer[incomingBufferSize++] = incomingByte;
00763             incomingBufferFree = OSCPREALLOCATEIZE;
00764         } else {
00765             error = ALLOCFAILED;
00766         }
00767     }
00768 }
00769 
00770 void OSCMessage::clearIncomingBuffer(){
00771     incomingBuffer = (uint8_t *) realloc ( incomingBuffer, OSCPREALLOCATEIZE);
00772     if (incomingBuffer != NULL){
00773         incomingBufferFree = OSCPREALLOCATEIZE;
00774     } else {
00775         error = ALLOCFAILED;
00776         incomingBuffer = NULL;
00777 
00778     }
00779     incomingBufferSize = 0;
00780 }