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 }