| #include <cstdlib>
|
| #include <cstring>
|
| #include <cmath>
|
| #include <cfloat>
|
| #include <limits>
|
| #include <emmintrin.h>
|
| #include <cstdio>
|
| #include <sstream>
|
| #include "stc_embed_c.h"
|
|
|
|
|
| void *aligned_malloc( unsigned int bytes, int align ) {
|
| int shift;
|
| char *temp = (char *) malloc( bytes + align );
|
|
|
| if ( temp == NULL ) return temp;
|
| shift = align - (int) (((unsigned long long) temp) & (align - 1));
|
| temp = temp + shift;
|
| temp[-1] = shift;
|
| return (void *) temp;
|
| }
|
|
|
|
|
|
|
| void aligned_free( void *vptr ) {
|
| char *ptr = (char *) vptr;
|
| free( ptr - ptr[-1] );
|
| return;
|
| }
|
|
|
|
|
|
|
| inline __m128i maxLessThan255( const __m128i v1, const __m128i v2 ) {
|
| register __m128i mask = _mm_set1_epi32( 0xffffffff );
|
| return _mm_max_epu8( _mm_andnot_si128( _mm_cmpeq_epi8( v1, mask ), v1 ), _mm_andnot_si128( _mm_cmpeq_epi8( v2, mask ), v2 ) );
|
| }
|
|
|
|
|
|
|
| inline u8 max16B( __m128i maxp ) {
|
| u8 mtemp[4];
|
| maxp = _mm_max_epu8( maxp, _mm_srli_si128(maxp, 8) );
|
| maxp = _mm_max_epu8( maxp, _mm_srli_si128(maxp, 4) );
|
| *((int*) mtemp) = _mm_cvtsi128_si32( maxp );
|
| if ( mtemp[2] > mtemp[0] ) mtemp[0] = mtemp[2];
|
| if ( mtemp[3] > mtemp[1] ) mtemp[1] = mtemp[3];
|
| if ( mtemp[1] > mtemp[0] ) return mtemp[1];
|
| else return mtemp[0];
|
| }
|
|
|
|
|
|
|
| inline u8 min16B( __m128i minp ) {
|
| u8 mtemp[4];
|
| minp = _mm_min_epu8( minp, _mm_srli_si128(minp, 8) );
|
| minp = _mm_min_epu8( minp, _mm_srli_si128(minp, 4) );
|
| *((int*) mtemp) = _mm_cvtsi128_si32( minp );
|
| if ( mtemp[2] < mtemp[0] ) mtemp[0] = mtemp[2];
|
| if ( mtemp[3] < mtemp[1] ) mtemp[1] = mtemp[3];
|
| if ( mtemp[1] < mtemp[0] ) return mtemp[1];
|
| else return mtemp[0];
|
| }
|
|
|
|
|
|
|
| double stc_embed( const u8 *vector, int vectorlength, const u8 *syndrome, int syndromelength, const void *pricevectorv, bool usefloat,
|
| u8 *stego, int matrixheight ) {
|
| int height, i, k, l, index, index2, parts, m, sseheight, altm, pathindex;
|
| u32 column, colmask, state;
|
| double totalprice;
|
|
|
| u8 *ssedone;
|
| u32 *path, *columns[2];
|
| int *matrices, *widths;
|
|
|
| if ( matrixheight > 31 ) throw stc_exception( "Submatrix height must not exceed 31.", 1 );
|
|
|
| height = 1 << matrixheight;
|
| colmask = height - 1;
|
| height = (height + 31) & (~31);
|
|
|
| parts = height >> 5;
|
|
|
| if ( stego != NULL ) {
|
| path = (u32*) malloc( vectorlength * parts * sizeof(u32) );
|
| if ( path == NULL ) {
|
| std::stringstream ss;
|
| ss << "Not enough memory (" << (unsigned int) (vectorlength * parts * sizeof(u32)) << " byte array could not be allocated).";
|
| throw stc_exception( ss.str(), 2 );
|
| }
|
| pathindex = 0;
|
| }
|
|
|
| {
|
| int shorter, longer, worm;
|
| double invalpha;
|
|
|
| matrices = (int *) malloc( syndromelength * sizeof(int) );
|
| widths = (int *) malloc( syndromelength * sizeof(int) );
|
|
|
| invalpha = (double) vectorlength / syndromelength;
|
| if ( invalpha < 1 ) {
|
| free( matrices );
|
| free( widths );
|
| if ( stego != NULL ) free( path );
|
| throw stc_exception( "The message cannot be longer than the cover object.", 3 );
|
| }
|
| |
| |
| |
| |
|
|
| shorter = (int) floor( invalpha );
|
| longer = (int) ceil( invalpha );
|
| if ( (columns[0] = getMatrix( shorter, matrixheight )) == NULL ) {
|
| free( matrices );
|
| free( widths );
|
| if ( stego != NULL ) free( path );
|
| return -1;
|
| }
|
| if ( (columns[1] = getMatrix( longer, matrixheight )) == NULL ) {
|
| free( columns[0] );
|
| free( matrices );
|
| free( widths );
|
| if ( stego != NULL ) free( path );
|
| return -1;
|
| }
|
| worm = 0;
|
| for ( i = 0; i < syndromelength; i++ ) {
|
| if ( worm + longer <= (i + 1) * invalpha + 0.5 ) {
|
| matrices[i] = 1;
|
| widths[i] = longer;
|
| worm += longer;
|
| } else {
|
| matrices[i] = 0;
|
| widths[i] = shorter;
|
| worm += shorter;
|
| }
|
| }
|
| }
|
|
|
| if ( usefloat ) {
|
| |
| |
|
|
| int pathindex8 = 0;
|
| int shift[2] = { 0, 4 };
|
| u8 mask[2] = { 0xf0, 0x0f };
|
| float *prices;
|
| u8 *path8 = (u8*) path;
|
| double *pricevector = (double*) pricevectorv;
|
| double total = 0;
|
| float inf = std::numeric_limits< float >::infinity();
|
|
|
| sseheight = height >> 2;
|
| ssedone = (u8*) malloc( sseheight * sizeof(u8) );
|
| prices = (float*) aligned_malloc( height * sizeof(float), 16 );
|
|
|
| {
|
| __m128 fillval = _mm_set1_ps( inf );
|
| for ( i = 0; i < height; i += 4 ) {
|
| _mm_store_ps( &prices[i], fillval );
|
| ssedone[i >> 2] = 0;
|
| }
|
| }
|
|
|
| prices[0] = 0.0f;
|
|
|
| for ( index = 0, index2 = 0; index2 < syndromelength; index2++ ) {
|
| register __m128 c1, c2;
|
|
|
| for ( k = 0; k < widths[index2]; k++, index++ ) {
|
| column = columns[matrices[index2]][k] & colmask;
|
|
|
| if ( vector[index] == 0 ) {
|
| c1 = _mm_setzero_ps();
|
| c2 = _mm_set1_ps( (float) pricevector[index] );
|
| } else {
|
| c1 = _mm_set1_ps( (float) pricevector[index] );
|
| c2 = _mm_setzero_ps();
|
| }
|
|
|
| total += pricevector[index];
|
|
|
| for ( m = 0; m < sseheight; m++ ) {
|
| if ( !ssedone[m] ) {
|
| register __m128 v1, v2, v3, v4;
|
| altm = (m ^ (column >> 2));
|
| v1 = _mm_load_ps( &prices[m << 2] );
|
| v2 = _mm_load_ps( &prices[altm << 2] );
|
| v3 = v1;
|
| v4 = v2;
|
| ssedone[m] = 1;
|
| ssedone[altm] = 1;
|
| switch ( column & 3 ) {
|
| case 0:
|
| break;
|
| case 1:
|
| v2 = _mm_shuffle_ps(v2, v2, 0xb1);
|
| v3 = _mm_shuffle_ps(v3, v3, 0xb1);
|
| break;
|
| case 2:
|
| v2 = _mm_shuffle_ps(v2, v2, 0x4e);
|
| v3 = _mm_shuffle_ps(v3, v3, 0x4e);
|
| break;
|
| case 3:
|
| v2 = _mm_shuffle_ps(v2, v2, 0x1b);
|
| v3 = _mm_shuffle_ps(v3, v3, 0x1b);
|
| break;
|
| }
|
| v1 = _mm_add_ps( v1, c1 );
|
| v2 = _mm_add_ps( v2, c2 );
|
| v3 = _mm_add_ps( v3, c2 );
|
| v4 = _mm_add_ps( v4, c1 );
|
|
|
| v1 = _mm_min_ps( v1, v2 );
|
| v4 = _mm_min_ps( v3, v4 );
|
|
|
| _mm_store_ps( &prices[m << 2], v1 );
|
| _mm_store_ps( &prices[altm << 2], v4 );
|
|
|
| if ( stego != NULL ) {
|
| v2 = _mm_cmpeq_ps( v1, v2 );
|
| v3 = _mm_cmpeq_ps( v3, v4 );
|
| path8[pathindex8 + (m >> 1)] = (path8[pathindex8 + (m >> 1)] & mask[m & 1]) | (_mm_movemask_ps( v2 ) << shift[m
|
| & 1]);
|
| path8[pathindex8 + (altm >> 1)] = (path8[pathindex8 + (altm >> 1)] & mask[altm & 1]) | (_mm_movemask_ps( v3 )
|
| << shift[altm & 1]);
|
| }
|
| }
|
| }
|
|
|
| for ( i = 0; i < sseheight; i++ ) {
|
| ssedone[i] = 0;
|
| }
|
|
|
| pathindex += parts;
|
| pathindex8 += parts << 2;
|
| }
|
|
|
| if ( syndrome[index2] == 0 ) {
|
| for ( i = 0, l = 0; i < sseheight; i += 2, l += 4 ) {
|
| _mm_store_ps( &prices[l], _mm_shuffle_ps(_mm_load_ps(&prices[i << 2]), _mm_load_ps(&prices[(i + 1) << 2]), 0x88) );
|
| }
|
| } else {
|
| for ( i = 0, l = 0; i < sseheight; i += 2, l += 4 ) {
|
| _mm_store_ps( &prices[l], _mm_shuffle_ps(_mm_load_ps(&prices[i << 2]), _mm_load_ps(&prices[(i + 1) << 2]), 0xdd) );
|
| }
|
| }
|
|
|
| if ( syndromelength - index2 <= matrixheight ) colmask >>= 1;
|
|
|
| {
|
| register __m128 fillval = _mm_set1_ps( inf );
|
| for ( l >>= 2; l < sseheight; l++ ) {
|
| _mm_store_ps( &prices[l << 2], fillval );
|
| }
|
| }
|
| }
|
|
|
| totalprice = prices[0];
|
|
|
| aligned_free( prices );
|
| free( ssedone );
|
|
|
| if ( totalprice >= total ) {
|
| free( matrices );
|
| free( widths );
|
| free( columns[0] );
|
| free( columns[1] );
|
| if ( stego != NULL ) free( path );
|
| throw stc_exception( "No solution exist.", 4 );
|
| }
|
| } else {
|
| |
| |
|
|
| int pathindex16 = 0, subprice = 0;
|
| u8 maxc = 0, minc = 0;
|
| u8 *prices, *pricevector = (u8*) pricevectorv;
|
| u16 *path16 = (u16 *) path;
|
| __m128i *prices16B;
|
|
|
| sseheight = height >> 4;
|
| ssedone = (u8*) malloc( sseheight * sizeof(u8) );
|
| prices = (u8*) aligned_malloc( height * sizeof(u8), 16 );
|
| prices16B = (__m128i *) prices;
|
|
|
| {
|
| __m128i napln = _mm_set1_epi32( 0xffffffff );
|
| for ( i = 0; i < sseheight; i++ ) {
|
| _mm_store_si128( &prices16B[i], napln );
|
| ssedone[i] = 0;
|
| }
|
| }
|
|
|
| prices[0] = 0;
|
|
|
| for ( index = 0, index2 = 0; index2 < syndromelength; index2++ ) {
|
| register __m128i c1, c2, maxp, minp;
|
|
|
| if ( (u32) maxc + pricevector[index] >= 254 ) {
|
| aligned_free( path );
|
| free( ssedone );
|
| free( matrices );
|
| free( widths );
|
| free( columns[0] );
|
| free( columns[1] );
|
| if ( stego != NULL ) free( path );
|
| throw stc_exception( "Price vector limit exceeded.", 5 );
|
| }
|
|
|
| for ( k = 0; k < widths[index2]; k++, index++ ) {
|
| column = columns[matrices[index2]][k] & colmask;
|
|
|
| if ( vector[index] == 0 ) {
|
| c1 = _mm_setzero_si128();
|
| c2 = _mm_set1_epi8( pricevector[index] );
|
| } else {
|
| c1 = _mm_set1_epi8( pricevector[index] );
|
| c2 = _mm_setzero_si128();
|
| }
|
|
|
| minp = _mm_set1_epi8( -1 );
|
| maxp = _mm_setzero_si128();
|
|
|
| for ( m = 0; m < sseheight; m++ ) {
|
| if ( !ssedone[m] ) {
|
| register __m128i v1, v2, v3, v4;
|
| altm = (m ^ (column >> 4));
|
| v1 = _mm_load_si128( &prices16B[m] );
|
| v2 = _mm_load_si128( &prices16B[altm] );
|
| v3 = v1;
|
| v4 = v2;
|
| ssedone[m] = 1;
|
| ssedone[altm] = 1;
|
| if ( column & 8 ) {
|
| v2 = _mm_shuffle_epi32(v2, 0x4e);
|
| v3 = _mm_shuffle_epi32(v3, 0x4e);
|
| }
|
| if ( column & 4 ) {
|
| v2 = _mm_shuffle_epi32(v2, 0xb1);
|
| v3 = _mm_shuffle_epi32(v3, 0xb1);
|
| }
|
| if ( column & 2 ) {
|
| v2 = _mm_shufflehi_epi16(v2, 0xb1);
|
| v3 = _mm_shufflehi_epi16(v3, 0xb1);
|
| v2 = _mm_shufflelo_epi16(v2, 0xb1);
|
| v3 = _mm_shufflelo_epi16(v3, 0xb1);
|
| }
|
| if ( column & 1 ) {
|
| v2 = _mm_or_si128( _mm_srli_epi16( v2, 8 ), _mm_slli_epi16( v2, 8 ) );
|
| v3 = _mm_or_si128( _mm_srli_epi16( v3, 8 ), _mm_slli_epi16( v3, 8 ) );
|
| }
|
| v1 = _mm_adds_epu8( v1, c1 );
|
| v2 = _mm_adds_epu8( v2, c2 );
|
| v3 = _mm_adds_epu8( v3, c2 );
|
| v4 = _mm_adds_epu8( v4, c1 );
|
|
|
| v1 = _mm_min_epu8( v1, v2 );
|
| v4 = _mm_min_epu8( v3, v4 );
|
|
|
| _mm_store_si128( &prices16B[m], v1 );
|
| _mm_store_si128( &prices16B[altm], v4 );
|
|
|
| minp = _mm_min_epu8( minp, _mm_min_epu8( v1, v4 ) );
|
| maxp = _mm_max_epu8( maxp, maxLessThan255( v1, v4 ) );
|
|
|
| if ( stego != NULL ) {
|
| v2 = _mm_cmpeq_epi8( v1, v2 );
|
| v3 = _mm_cmpeq_epi8( v3, v4 );
|
| path16[pathindex16 + m] = (u16) _mm_movemask_epi8( v2 );
|
| path16[pathindex16 + altm] = (u16) _mm_movemask_epi8( v3 );
|
| }
|
| }
|
| }
|
|
|
| maxc = max16B( maxp );
|
| minc = min16B( minp );
|
|
|
| maxc -= minc;
|
| subprice += minc;
|
| {
|
| register __m128i mask = _mm_set1_epi32( 0xffffffff );
|
| register __m128i m = _mm_set1_epi8( minc );
|
| for ( i = 0; i < sseheight; i++ ) {
|
| register __m128i res;
|
| register __m128i pr = prices16B[i];
|
| res = _mm_andnot_si128( _mm_cmpeq_epi8( pr, mask ), m );
|
| prices16B[i] = _mm_sub_epi8( pr, res );
|
| ssedone[i] = 0;
|
| }
|
| }
|
|
|
| pathindex += parts;
|
| pathindex16 += parts << 1;
|
| }
|
|
|
| {
|
| register __m128i mask = _mm_set1_epi32( 0x00ff00ff );
|
|
|
| if ( minc == 255 ) {
|
| aligned_free( path );
|
| free( ssedone );
|
| free( matrices );
|
| free( widths );
|
| free( columns[0] );
|
| free( columns[1] );
|
| if ( stego != NULL ) free( path );
|
| throw stc_exception( "The syndrome is not in the syndrome matrix range.", 4 );
|
| }
|
|
|
| if ( syndrome[index2] == 0 ) {
|
| for ( i = 0, l = 0; i < sseheight; i += 2, l++ ) {
|
| _mm_store_si128( &prices16B[l], _mm_packus_epi16( _mm_and_si128( _mm_load_si128( &prices16B[i] ), mask ),
|
| _mm_and_si128( _mm_load_si128( &prices16B[i + 1] ), mask ) ) );
|
| }
|
| } else {
|
| for ( i = 0, l = 0; i < sseheight; i += 2, l++ ) {
|
| _mm_store_si128( &prices16B[l], _mm_packus_epi16( _mm_and_si128( _mm_srli_si128(_mm_load_si128(&prices16B[i]), 1),
|
| mask ), _mm_and_si128( _mm_srli_si128(_mm_load_si128(&prices16B[i + 1]), 1), mask ) ) );
|
| }
|
| }
|
|
|
| if ( syndromelength - index2 <= matrixheight ) colmask >>= 1;
|
|
|
| register __m128i fillval = _mm_set1_epi32( 0xffffffff );
|
| for ( ; l < sseheight; l++ )
|
| _mm_store_si128( &prices16B[l], fillval );
|
| }
|
| }
|
|
|
| totalprice = subprice + prices[0];
|
|
|
| aligned_free( prices );
|
| free( ssedone );
|
| }
|
|
|
| if ( stego != NULL ) {
|
| pathindex -= parts;
|
| index--;
|
| index2--;
|
| state = 0;
|
|
|
|
|
|
|
| state = 0;
|
| colmask = 0;
|
| for ( ; index2 >= 0; index2-- ) {
|
| for ( k = widths[index2] - 1; k >= 0; k--, index-- ) {
|
| if ( k == widths[index2] - 1 ) {
|
| state = (state << 1) | syndrome[index2];
|
| if ( syndromelength - index2 <= matrixheight ) colmask = (colmask << 1) | 1;
|
| }
|
|
|
| if ( path[pathindex + (state >> 5)] & (1 << (state & 31)) ) {
|
| stego[index] = 1;
|
| state = state ^ (columns[matrices[index2]][k] & colmask);
|
| } else {
|
| stego[index] = 0;
|
| }
|
|
|
| pathindex -= parts;
|
| }
|
| }
|
| free( path );
|
| }
|
|
|
| free( matrices );
|
| free( widths );
|
| free( columns[0] );
|
| free( columns[1] );
|
|
|
| return totalprice;
|
| }
|
|
|
|
|
