00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 #include <cmath>
00011 #include <cstdlib>
00012 #include <cstdio>
00013 #include <cstring>
00014 #include <pion/algorithm.hpp>
00015 #include <boost/assert.hpp>
00016 
00017 
00018 #define SHIFT_BITMASK(ptr, mask)    if (mask & 0x01) { mask = 0x80; ++ptr; } else mask >>= 1;
00019 
00020 
00021 namespace pion {        
00022 
00023 
00024 bool algorithm::base64_decode(const std::string &input, std::string &output)
00025 {
00026     static const char nop = -1; 
00027     static const char decoding_data[] = {
00028         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00029         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00030         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop, 62, nop,nop,nop, 63,
00031         52, 53, 54,  55,  56, 57, 58, 59,  60, 61,nop,nop, nop,nop,nop,nop,
00032         nop, 0,  1,   2,   3,  4,  5,  6,   7,  8,  9, 10,  11, 12, 13, 14,
00033         15, 16, 17,  18,  19, 20, 21, 22,  23, 24, 25,nop, nop,nop,nop,nop,
00034         nop,26, 27,  28,  29, 30, 31, 32,  33, 34, 35, 36,  37, 38, 39, 40,
00035         41, 42, 43,  44,  45, 46, 47, 48,  49, 50, 51,nop, nop,nop,nop,nop,
00036         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00037         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00038         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00039         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00040         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00041         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00042         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop,
00043         nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop, nop,nop,nop,nop
00044         };
00045 
00046     unsigned int input_length=input.size();
00047     const char * input_ptr = input.data();
00048 
00049     
00050     output.clear();
00051     output.reserve(((input_length+2)/3)*4);
00052 
00053     
00054     
00055 
00056     for (unsigned int i=0; i<input_length;i++) {
00057         char base64code0;
00058         char base64code1;
00059         char base64code2 = 0;   
00060         char base64code3;
00061 
00062         base64code0 = decoding_data[static_cast<int>(input_ptr[i])];
00063         if(base64code0==nop)            
00064             return false;
00065         if(!(++i<input_length)) 
00066             return false;
00067         base64code1 = decoding_data[static_cast<int>(input_ptr[i])];
00068         if(base64code1==nop)            
00069             return false;
00070 
00071         output += ((base64code0 << 2) | ((base64code1 >> 4) & 0x3));
00072 
00073         if(++i<input_length) {
00074             char c = input_ptr[i];
00075             if(c =='=') { 
00076                 BOOST_ASSERT( (base64code1 & 0x0f)==0);
00077                 return true;
00078             }
00079             base64code2 = decoding_data[static_cast<int>(input_ptr[i])];
00080             if(base64code2==nop)            
00081                 return false;
00082 
00083             output += ((base64code1 << 4) & 0xf0) | ((base64code2 >> 2) & 0x0f);
00084         }
00085 
00086         if(++i<input_length) {
00087             char c = input_ptr[i];
00088             if(c =='=') { 
00089                 BOOST_ASSERT( (base64code2 & 0x03)==0);
00090                 return true;
00091             }
00092             base64code3 = decoding_data[static_cast<int>(input_ptr[i])];
00093             if(base64code3==nop)            
00094                 return false;
00095 
00096             output += (((base64code2 << 6) & 0xc0) | base64code3 );
00097         }
00098 
00099     }
00100 
00101     return true;
00102 }
00103 
00104 bool algorithm::base64_encode(const std::string &input, std::string &output)
00105 {
00106     static const char encoding_data[] = 
00107         "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
00108 
00109     unsigned int input_length=input.size();
00110     const char * input_ptr = input.data();
00111 
00112     
00113     output.clear();
00114     output.reserve(((input_length+2)/3)*4);
00115 
00116     
00117     
00118     
00119     for (unsigned int i=0; i<input_length;i++) {
00120         int base64code0=0;
00121         int base64code1=0;
00122         int base64code2=0;
00123         int base64code3=0;
00124 
00125         base64code0 = (input_ptr[i] >> 2)  & 0x3f;  
00126         output += encoding_data[base64code0];
00127         base64code1 = (input_ptr[i] << 4 ) & 0x3f;  
00128 
00129         if (++i < input_length) {
00130             base64code1 |= (input_ptr[i] >> 4) & 0x0f; 
00131             output += encoding_data[base64code1];
00132             base64code2 = (input_ptr[i] << 2) & 0x3f;  
00133 
00134             if (++i < input_length) {
00135                 base64code2 |= (input_ptr[i] >> 6) & 0x03; 
00136                 base64code3  = input_ptr[i] & 0x3f;       
00137                 output += encoding_data[base64code2];
00138                 output += encoding_data[base64code3];
00139             } else {
00140                 output += encoding_data[base64code2];
00141                 output += '=';
00142             }
00143         } else {
00144             output += encoding_data[base64code1];
00145             output += '=';
00146             output += '=';
00147         }
00148     }
00149 
00150     return true;
00151 }
00152 
00153 std::string algorithm::url_decode(const std::string& str)
00154 {
00155     char decode_buf[3];
00156     std::string result;
00157     result.reserve(str.size());
00158     
00159     for (std::string::size_type pos = 0; pos < str.size(); ++pos) {
00160         switch(str[pos]) {
00161         case '+':
00162             
00163             result += ' ';
00164             break;
00165         case '%':
00166             
00167             if (pos + 2 < str.size()) {
00168                 decode_buf[0] = str[++pos];
00169                 decode_buf[1] = str[++pos];
00170                 decode_buf[2] = '\0';
00171                 result += static_cast<char>( strtol(decode_buf, 0, 16) );
00172             } else {
00173                 
00174                 result += '%';
00175             }
00176             break;
00177         default:
00178             
00179             result += str[pos];
00180         }
00181     };
00182     
00183     return result;
00184 }
00185     
00186 std::string algorithm::url_encode(const std::string& str)
00187 {
00188     char encode_buf[4];
00189     std::string result;
00190     encode_buf[0] = '%';
00191     result.reserve(str.size());
00192 
00193     
00194     
00195     
00196     for (std::string::size_type pos = 0; pos < str.size(); ++pos) {
00197         switch(str[pos]) {
00198         default:
00199             if (str[pos] > 32 && str[pos] < 127) {
00200                 
00201                 result += str[pos];
00202                 break;
00203             }
00204             
00205         case ' ':   
00206         case '$': case '&': case '+': case ',': case '/': case ':':
00207         case ';': case '=': case '?': case '@': case '"': case '<':
00208         case '>': case '#': case '%': case '{': case '}': case '|':
00209         case '\\': case '^': case '~': case '[': case ']': case '`':
00210             
00211             sprintf(encode_buf+1, "%.2X", (unsigned char)(str[pos]));
00212             result += encode_buf;
00213             break;
00214         }
00215     };
00216     
00217     return result;
00218 }
00219 
00220 
00221 
00222 
00223 
00224 
00225 std::string algorithm::xml_encode(const std::string& str)
00226 {
00227     std::string result;
00228     result.reserve(str.size() + 20);    
00229     const unsigned char *ptr = reinterpret_cast<const unsigned char*>(str.c_str());
00230     const unsigned char *end_ptr = ptr + str.size();
00231     while (ptr < end_ptr) {
00232         
00233         
00234         
00235         
00236         if ((*ptr >= 0x20 && *ptr <= 0x7F) || *ptr == 0x9 || *ptr == 0xa || *ptr == 0xd) {
00237             
00238             switch(*ptr) {
00239                     
00240                 case '&':
00241                     result += "&";
00242                     break;
00243                 case '<':
00244                     result += "<";
00245                     break;
00246                 case '>':
00247                     result += ">";
00248                     break;
00249                 case '\"':
00250                     result += """;
00251                     break;
00252                 case '\'':
00253                     result += "'";
00254                     break;
00255                 default:
00256                     result += *ptr;
00257             }
00258         } else if (*ptr >= 0xC2 && *ptr <= 0xDF) {
00259             
00260             if (*(ptr+1) >= 0x80 && *(ptr+1) <= 0xBF) {
00261                 result += *ptr;
00262                 result += *(++ptr);
00263             } else {
00264                 
00265                 result += 0xef;
00266                 result += 0xbf;
00267                 result += 0xbd;
00268             }
00269         } else if (*ptr >= 0xE0 && *ptr <= 0xEF) {
00270             
00271             if (*(ptr+1) >= 0x80 && *(ptr+1) <= 0xBF
00272                 && *(ptr+2) >= 0x80 && *(ptr+2) <= 0xBF) {
00273                 result += *ptr;
00274                 result += *(++ptr);
00275                 result += *(++ptr);
00276             } else {
00277                 
00278                 result += 0xef;
00279                 result += 0xbf;
00280                 result += 0xbd;
00281             }
00282         } else if (*ptr >= 0xF0 && *ptr <= 0xF4) {
00283             
00284             if (*(ptr+1) >= 0x80 && *(ptr+1) <= 0xBF
00285                 && *(ptr+2) >= 0x80 && *(ptr+2) <= 0xBF
00286                 && *(ptr+3) >= 0x80 && *(ptr+3) <= 0xBF) {
00287                 result += *ptr;
00288                 result += *(++ptr);
00289                 result += *(++ptr);
00290                 result += *(++ptr);
00291             } else {
00292                 
00293                 result += 0xef;
00294                 result += 0xbf;
00295                 result += 0xbd;
00296             }
00297         } else {
00298             
00299             result += 0xef;
00300             result += 0xbf;
00301             result += 0xbd;
00302         }
00303         ++ptr;
00304     }
00305     
00306     return result;
00307 }
00308 
00309 void algorithm::float_from_bytes(long double& value, const unsigned char *ptr, size_t num_exp_bits, size_t num_fraction_bits)
00310 {
00311     
00312     const int value_sign = (*ptr & 0x80) ? -1 : 1;
00313     
00314     
00315     unsigned char mask = 0x80;
00316     boost::int16_t exponent = 0;
00317     for (size_t n = 0; n < num_exp_bits; ++n) {
00318         SHIFT_BITMASK(ptr, mask);
00319         exponent *= 2;
00320         if (*ptr & mask)
00321             exponent += 1;
00322     }
00323     
00324     
00325     long double significand = exponent ? 1.0 : 0.0;
00326     long double significand_value = 1.0;
00327     while (num_fraction_bits) {
00328         SHIFT_BITMASK(ptr, mask);
00329         significand_value /= 2;
00330         if (*ptr & mask)
00331             significand += significand_value;
00332         --num_fraction_bits;
00333     }
00334     
00335     
00336     exponent -= (::pow((long double)2, (int)(num_exp_bits - 1)) - 1);
00337     value = value_sign * significand * ::pow((long double)2, exponent);
00338 }
00339 
00340 void algorithm::float_to_bytes(long double value, unsigned char *buf, size_t num_exp_bits, size_t num_fraction_bits)
00341 {
00342     
00343     unsigned char *ptr = buf;
00344     memset(ptr, 0x00, ::ceil(static_cast<float>(num_exp_bits + num_fraction_bits + 1) / 8));
00345     
00346     
00347     if (value < 0) {
00348         *ptr = 0x80;
00349         value *= -1;
00350     }
00351     
00352     
00353     boost::int16_t exponent = 0;
00354     while (value >= 1) {
00355         value /= 2;
00356         ++exponent;
00357     }
00358 
00359     
00360     unsigned char mask = 0x40;
00361     for (size_t n = num_exp_bits; n > 0; --n) {
00362         if (n >= 8) {
00363             ++ptr;
00364             n -= 7;
00365         } else {
00366             SHIFT_BITMASK(ptr, mask);
00367         }
00368     }
00369     
00370     
00371     bool got_exponent = false;
00372     boost::uint16_t num_bits = 0;
00373     while (value && num_bits < num_fraction_bits) {
00374         value *= 2;
00375         if (got_exponent) {
00376             if (value >= 1.0) {
00377                 *ptr |= mask;
00378                 value -= 1.0;
00379             }
00380             SHIFT_BITMASK(ptr, mask);
00381             ++num_bits;
00382         } else {
00383             --exponent;
00384             if (value >= 1.0) {
00385                 value -= 1.0;
00386                 got_exponent = true;
00387             }
00388         }
00389     }
00390     
00391     
00392     
00393     boost::int32_t high_bit = ::pow((long double)2, (int)(num_exp_bits - 1));
00394     if (got_exponent)
00395         exponent += (high_bit - 1);
00396     else
00397         exponent = 0;
00398     
00399     
00400     ptr = buf;
00401     mask = 0x80;
00402     for (size_t n = 0; n < num_exp_bits; ++n) {
00403         SHIFT_BITMASK(ptr, mask);
00404         if (exponent >= high_bit) {
00405             *ptr |= mask;
00406             exponent -= high_bit;
00407         }
00408         high_bit /= 2;
00409     }
00410 }
00411     
00412 }