wufuc/wufuc/patternfind.c

#include "patternfind.h"

#include "rtl_malloc.h"

#include <stddef.h>
#include <stdbool.h>
#include <string.h>

static inline bool isHex(char ch)
{
        return (ch >= '0' && ch <= '9') || (ch >= 'A' && ch <= 'F') || (ch >= 'a' && ch <= 'f');
}

static inline int hexchtoint(char ch)
{
        if ( ch >= '0' && ch <= '9' )
                return ch - '0';
        else if ( ch >= 'A' && ch <= 'F' )
                return ch - 'A' + 10;
        else if ( ch >= 'a' && ch <= 'f' )
                return ch - 'a' + 10;
        return -1;
}

static inline size_t formathexpattern(const char *patterntext, char *formattext, size_t formattextsize)
{
        size_t len = strlen(patterntext);
        size_t result = 0;
        for ( size_t i = 0; i < len; i++ ) {
                if ( patterntext[i] == '?' || hexchtoint(patterntext[i]) != -1 ) {
                        if ( formattext && result + 1 < formattextsize )
                                formattext[result] = patterntext[i];

                        result++;
                }
        }
        if ( result % 2 ) { //not a multiple of 2
                if ( formattext && result + 1 < formattextsize )
                        formattext[result] = '?';

                result++;
        }
        if ( formattext ) {
                if ( result <= formattextsize )
                        formattext[result] = '\0';
                else
                        formattext[0] = '\0';
        }
        return result;
}

bool patterntransform(const char *patterntext, PatternByte *pattern, size_t patternsize)
{
        memset(pattern, 0, patternsize * sizeof(PatternByte));
        size_t len = formathexpattern(patterntext, NULL, 0);

        if ( !len || len / 2 > patternsize )
                return false;

        size_t size = len + 1;
        char *formattext = rtl_malloc(size);
        formathexpattern(patterntext, formattext, size);
        PatternByte newByte;

        for ( int i = 0, j = 0, k = 0; i < len && k <= patternsize; i++ ) {
                if ( formattext[i] == '?' ) { //wildcard
                        newByte.nibble[j].wildcard = true; //match anything
                } else { //hex
                        newByte.nibble[j].wildcard = false;
                        newByte.nibble[j].data = hexchtoint(formattext[i]) & 0xF;
                }

                j++;
                if ( j == 2 ) { //two nibbles = one byte
                        j = 0;
                        pattern[k++] = newByte;
                }
        }
        return true;
}

static inline bool patternmatchbyte(unsigned char byte, const PatternByte pbyte)
{
        int matched = 0;

        unsigned char n1 = (byte >> 4) & 0xF;
        if ( pbyte.nibble[0].wildcard )
                matched++;
        else if ( pbyte.nibble[0].data == n1 )
                matched++;

        unsigned char n2 = byte & 0xF;
        if ( pbyte.nibble[1].wildcard )
                matched++;
        else if ( pbyte.nibble[1].data == n2 )
                matched++;

        return (matched == 2);
}

unsigned char *patternfind(unsigned char *data, size_t datasize, const char *pattern)
{
        size_t searchpatternsize = formathexpattern(pattern, NULL, 0) / 2;
        PatternByte *searchpattern = rtl_calloc(searchpatternsize, sizeof(PatternByte));

        unsigned char *result = NULL;
        if ( patterntransform(pattern, searchpattern, searchpatternsize) )
                result = patternfind3(data, datasize, searchpattern, searchpatternsize);
        
        rtl_free(searchpattern);
        return result;
}

unsigned char *patternfind2(unsigned char *data, size_t datasize, unsigned char *pattern, size_t patternsize)
{
        if ( patternsize > datasize )
                patternsize = datasize;
        for ( size_t i = 0, pos = 0; i < datasize; i++ ) {
                if ( data[i] == pattern[pos] ) {
                        pos++;
                        if ( pos == patternsize )
                                return &data[i - patternsize + 1];
                } else if ( pos > 0 ) {
                        i -= pos;
                        pos = 0; //reset current pattern position
                }
        }
        return NULL;
}

static inline void patternwritebyte(unsigned char *byte, const PatternByte pbyte)
{
        unsigned char n1 = (*byte >> 4) & 0xF;
        unsigned char n2 = *byte & 0xF;
        if ( !pbyte.nibble[0].wildcard )
                n1 = pbyte.nibble[0].data;
        if ( !pbyte.nibble[1].wildcard )
                n2 = pbyte.nibble[1].data;
        *byte = ((n1 << 4) & 0xF0) | (n2 & 0xF);
}

void patternwrite(unsigned char *data, size_t datasize, const char *pattern)
{
        size_t writepatternsize = formathexpattern(pattern, NULL, 0) / 2;
        PatternByte *writepattern = rtl_calloc(writepatternsize, sizeof(PatternByte));

        if ( patterntransform(pattern, writepattern, writepatternsize) ) {
                if ( writepatternsize > datasize )
                        writepatternsize = datasize;
                for ( size_t i = 0; i < writepatternsize; i++ )
                        patternwritebyte(&data[i], writepattern[i]);
        }

        rtl_free(writepattern);
}

bool patternsnr(unsigned char *data, size_t datasize, const char *searchpattern, const char *replacepattern)
{
        unsigned char *found = patternfind(data, datasize, searchpattern);
        if ( !found )
                return false;
        patternwrite(found, datasize - (found - data), replacepattern);
        return true;
}

unsigned char *patternfind3(unsigned char *data, size_t datasize, const PatternByte *pattern, size_t searchpatternsize)
{
        for ( size_t i = 0, pos = 0; i < datasize; i++ ) { //search for the pattern
                if ( patternmatchbyte(data[i], pattern[pos]) ) { //check if our pattern matches the current byte
                        pos++;
                        if ( pos == searchpatternsize ) //everything matched
                                return &data[i - searchpatternsize + 1];
                } else if ( pos > 0 ) { //fix by Computer_Angel
                        i -= pos;
                        pos = 0; //reset current pattern position
                }
        }
        return NULL;
}