Написать собственную реализацию стандартной функции strstr (исправить)

#include <iostream>
#include <string.h>
#include <stdio.h>
using namespace std;
 
char*  my_strstr (const char *str1, const char *str2)
{
    
        char *cp = (char *) str1;
        char *s1, *s2;
    
        if ( !*str2 )
            return((char *)str1);
 
        while (*cp)
        {
                s1 = cp;
                s2 = (char *) str2;
 
                while ( *s1 && *s2 && !(*s1-*s2) )
                        s1++, s2++;
 
                if (!*s2)
                        return(cp);
 
                cp++;
        }
 
        return(NULL);
 
}
 
int main()
{
 
    char s1[] = "aaab";
    char f1[] = "aab";
    char f2[] = "aaa";
    char s2[] = "aab ggf";
    char f3[] = " g";
    char s3[] = "a";
 
 
    if (strstr(s1, f1) != my_strstr(s1, f1))
        printf("test1 fail ", strstr(s1, f1), my_strstr(s1, f1));
    else
        printf(".");
    
 
    if (strstr(s1, f2) != my_strstr(s1, f2))
        printf("test2 fail", strstr(s1, f2), my_strstr(s1, f2));
    else
        printf(".");
    
 
    if (strstr(s2, f3) != my_strstr(s2, f3))
        printf("test5 fail ", strstr(s2, f3), my_strstr(s2, f3));
    else
        printf(".");
 
 
    if (strstr(s3, f1) != my_strstr(s3, f1))
        printf("tst5 fail", strstr(s3, f1), my_strstr(s3, f1));
    else
        printf(" ");
 
 
    printf(" ");
 
    system("pause");
    return 0;
}