mknkboot/beastpatcher: implement basic firmware validation

This imports the MD5 code used by other utilities and creates
a function for checking the provided NK.bin against known original
firmware checksums. Integration into mknkboot and beastpatcher is
also added.

For the sake of consistency with beastpatcher, mknkboot had its
printf statements rewrote to print to stderr like beastpatcher
does.

Change-Id: I0e52271d8d627a5b02302ab5cd1da2815b7cec1e

6 files changed, 329 insertions, 14 deletions

diff --git a/utils/MTP/beastpatcher/Makefile b/utils/MTP/beastpatcher/Makefile
index 74cc46bf44..296e4ffb4e 100644
--- a/utils/MTP/beastpatcher/Makefile
+++ b/utils/MTP/beastpatcher/Makefile
@@ -33,9 +33,9 @@ CC = $(CROSS)gcc
 
 all: $(OUTPUT)
 
-SOURCES = beastpatcher.c bootimg.c mknkboot.c main.c
-HEADERS = beastpatcher.h mtp_common.h bootimg.h mknkboot.h
-OBJS    = beastpatcher.o bootimg.o mknkboot.o main.o mtp_libmtp.o
+SOURCES = beastpatcher.c bootimg.c mknkboot.c md5.c main.c
+HEADERS = beastpatcher.h mtp_common.h bootimg.h mknkboot.h md5.h
+OBJS    = beastpatcher.o bootimg.o mknkboot.o md5.o main.o mtp_libmtp.o
 MTPSRCS_W32 = mtp_win32.c
 MTPSRCS_MTP = mtp_libmtp.c
 
@@ -71,7 +71,7 @@ bin2c: ../../../rbutil/tools/bin2c.c
 bootimg.c: bootloader.bin bin2c
 	./bin2c bootloader.bin bootimg
 
-mknkboot: mknkboot.c
+mknkboot: mknkboot.c md5.c
 	$(SILENT)$(NATIVECC) $(CFLAGS) $+ -o $@
 clean:
 	rm -f beastpatcher.exe beastpatcher-mac beastpatcher-i386 beastpatcher-ppc beastpatcher bin2c bootimg.c bootimg.h mknkboot *~ *.o beastpatcher.dmg
diff --git a/utils/MTP/beastpatcher/beastpatcher.c b/utils/MTP/beastpatcher/beastpatcher.c
index b266079af5..6f9dcb6b50 100644
--- a/utils/MTP/beastpatcher/beastpatcher.c
+++ b/utils/MTP/beastpatcher/beastpatcher.c
@@ -213,7 +213,7 @@ int beastpatcher(const char* bootfile, const char* firmfile, int interactive)
     {
         if(firmfile) {
             /* if a firmware file is given create a dualboot image. */
-            if(mknkboot(&firmware, &bootloader, &fw))
+            if(verifyfirm(&firmware) < 0 || mknkboot(&firmware, &bootloader, &fw))
             {
                 fprintf(stderr,"[ERR]  Creating dualboot firmware failed.\n");
                 return 1;
diff --git a/utils/MTP/beastpatcher/md5.c b/utils/MTP/beastpatcher/md5.c
new file mode 100644
index 0000000000..6c5e8127f9
--- /dev/null
+++ b/utils/MTP/beastpatcher/md5.c
@@ -0,0 +1,246 @@
+/*
+ *  RFC 1321 compliant MD5 implementation
+ *
+ *  Copyright (C) 2001-2003  Christophe Devine
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <string.h>
+
+#include "md5.h"
+
+#define GET_UINT32(n,b,i)                       \
+{                                               \
+    (n) = ( (uint32) (b)[(i)    ]       )       \
+        | ( (uint32) (b)[(i) + 1] <<  8 )       \
+        | ( (uint32) (b)[(i) + 2] << 16 )       \
+        | ( (uint32) (b)[(i) + 3] << 24 );      \
+}
+
+#define PUT_UINT32(n,b,i)                       \
+{                                               \
+    (b)[(i)    ] = (uint8) ( (n)       );       \
+    (b)[(i) + 1] = (uint8) ( (n) >>  8 );       \
+    (b)[(i) + 2] = (uint8) ( (n) >> 16 );       \
+    (b)[(i) + 3] = (uint8) ( (n) >> 24 );       \
+}
+
+void md5_starts( md5_context *ctx )
+{
+    ctx->total[0] = 0;
+    ctx->total[1] = 0;
+
+    ctx->state[0] = 0x67452301;
+    ctx->state[1] = 0xEFCDAB89;
+    ctx->state[2] = 0x98BADCFE;
+    ctx->state[3] = 0x10325476;
+}
+
+void md5_process( md5_context *ctx, uint8 data[64] )
+{
+    uint32 X[16], A, B, C, D;
+
+    GET_UINT32( X[0],  data,  0 );
+    GET_UINT32( X[1],  data,  4 );
+    GET_UINT32( X[2],  data,  8 );
+    GET_UINT32( X[3],  data, 12 );
+    GET_UINT32( X[4],  data, 16 );
+    GET_UINT32( X[5],  data, 20 );
+    GET_UINT32( X[6],  data, 24 );
+    GET_UINT32( X[7],  data, 28 );
+    GET_UINT32( X[8],  data, 32 );
+    GET_UINT32( X[9],  data, 36 );
+    GET_UINT32( X[10], data, 40 );
+    GET_UINT32( X[11], data, 44 );
+    GET_UINT32( X[12], data, 48 );
+    GET_UINT32( X[13], data, 52 );
+    GET_UINT32( X[14], data, 56 );
+    GET_UINT32( X[15], data, 60 );
+
+#define S(x,n) ((x << n) | ((x & 0xFFFFFFFF) >> (32 - n)))
+
+#define P(a,b,c,d,k,s,t)                                \
+{                                                       \
+    a += F(b,c,d) + X[k] + t; a = S(a,s) + b;           \
+}
+
+    A = ctx->state[0];
+    B = ctx->state[1];
+    C = ctx->state[2];
+    D = ctx->state[3];
+
+#define F(x,y,z) (z ^ (x & (y ^ z)))
+
+    P( A, B, C, D,  0,  7, 0xD76AA478 );
+    P( D, A, B, C,  1, 12, 0xE8C7B756 );
+    P( C, D, A, B,  2, 17, 0x242070DB );
+    P( B, C, D, A,  3, 22, 0xC1BDCEEE );
+    P( A, B, C, D,  4,  7, 0xF57C0FAF );
+    P( D, A, B, C,  5, 12, 0x4787C62A );
+    P( C, D, A, B,  6, 17, 0xA8304613 );
+    P( B, C, D, A,  7, 22, 0xFD469501 );
+    P( A, B, C, D,  8,  7, 0x698098D8 );
+    P( D, A, B, C,  9, 12, 0x8B44F7AF );
+    P( C, D, A, B, 10, 17, 0xFFFF5BB1 );
+    P( B, C, D, A, 11, 22, 0x895CD7BE );
+    P( A, B, C, D, 12,  7, 0x6B901122 );
+    P( D, A, B, C, 13, 12, 0xFD987193 );
+    P( C, D, A, B, 14, 17, 0xA679438E );
+    P( B, C, D, A, 15, 22, 0x49B40821 );
+
+#undef F
+
+#define F(x,y,z) (y ^ (z & (x ^ y)))
+
+    P( A, B, C, D,  1,  5, 0xF61E2562 );
+    P( D, A, B, C,  6,  9, 0xC040B340 );
+    P( C, D, A, B, 11, 14, 0x265E5A51 );
+    P( B, C, D, A,  0, 20, 0xE9B6C7AA );
+    P( A, B, C, D,  5,  5, 0xD62F105D );
+    P( D, A, B, C, 10,  9, 0x02441453 );
+    P( C, D, A, B, 15, 14, 0xD8A1E681 );
+    P( B, C, D, A,  4, 20, 0xE7D3FBC8 );
+    P( A, B, C, D,  9,  5, 0x21E1CDE6 );
+    P( D, A, B, C, 14,  9, 0xC33707D6 );
+    P( C, D, A, B,  3, 14, 0xF4D50D87 );
+    P( B, C, D, A,  8, 20, 0x455A14ED );
+    P( A, B, C, D, 13,  5, 0xA9E3E905 );
+    P( D, A, B, C,  2,  9, 0xFCEFA3F8 );
+    P( C, D, A, B,  7, 14, 0x676F02D9 );
+    P( B, C, D, A, 12, 20, 0x8D2A4C8A );
+
+#undef F
+    
+#define F(x,y,z) (x ^ y ^ z)
+
+    P( A, B, C, D,  5,  4, 0xFFFA3942 );
+    P( D, A, B, C,  8, 11, 0x8771F681 );
+    P( C, D, A, B, 11, 16, 0x6D9D6122 );
+    P( B, C, D, A, 14, 23, 0xFDE5380C );
+    P( A, B, C, D,  1,  4, 0xA4BEEA44 );
+    P( D, A, B, C,  4, 11, 0x4BDECFA9 );
+    P( C, D, A, B,  7, 16, 0xF6BB4B60 );
+    P( B, C, D, A, 10, 23, 0xBEBFBC70 );
+    P( A, B, C, D, 13,  4, 0x289B7EC6 );
+    P( D, A, B, C,  0, 11, 0xEAA127FA );
+    P( C, D, A, B,  3, 16, 0xD4EF3085 );
+    P( B, C, D, A,  6, 23, 0x04881D05 );
+    P( A, B, C, D,  9,  4, 0xD9D4D039 );
+    P( D, A, B, C, 12, 11, 0xE6DB99E5 );
+    P( C, D, A, B, 15, 16, 0x1FA27CF8 );
+    P( B, C, D, A,  2, 23, 0xC4AC5665 );
+
+#undef F
+
+#define F(x,y,z) (y ^ (x | ~z))
+
+    P( A, B, C, D,  0,  6, 0xF4292244 );
+    P( D, A, B, C,  7, 10, 0x432AFF97 );
+    P( C, D, A, B, 14, 15, 0xAB9423A7 );
+    P( B, C, D, A,  5, 21, 0xFC93A039 );
+    P( A, B, C, D, 12,  6, 0x655B59C3 );
+    P( D, A, B, C,  3, 10, 0x8F0CCC92 );
+    P( C, D, A, B, 10, 15, 0xFFEFF47D );
+    P( B, C, D, A,  1, 21, 0x85845DD1 );
+    P( A, B, C, D,  8,  6, 0x6FA87E4F );
+    P( D, A, B, C, 15, 10, 0xFE2CE6E0 );
+    P( C, D, A, B,  6, 15, 0xA3014314 );
+    P( B, C, D, A, 13, 21, 0x4E0811A1 );
+    P( A, B, C, D,  4,  6, 0xF7537E82 );
+    P( D, A, B, C, 11, 10, 0xBD3AF235 );
+    P( C, D, A, B,  2, 15, 0x2AD7D2BB );
+    P( B, C, D, A,  9, 21, 0xEB86D391 );
+
+#undef F
+
+    ctx->state[0] += A;
+    ctx->state[1] += B;
+    ctx->state[2] += C;
+    ctx->state[3] += D;
+}
+
+void md5_update( md5_context *ctx, uint8 *input, uint32 length )
+{
+    uint32 left, fill;
+
+    if( ! length ) return;
+
+    left = ctx->total[0] & 0x3F;
+    fill = 64 - left;
+
+    ctx->total[0] += length;
+    ctx->total[0] &= 0xFFFFFFFF;
+
+    if( ctx->total[0] < length )
+        ctx->total[1]++;
+
+    if( left && length >= fill )
+    {
+        memcpy( (void *) (ctx->buffer + left),
+                (void *) input, fill );
+        md5_process( ctx, ctx->buffer );
+        length -= fill;
+        input  += fill;
+        left = 0;
+    }
+
+    while( length >= 64 )
+    {
+        md5_process( ctx, input );
+        length -= 64;
+        input  += 64;
+    }
+
+    if( length )
+    {
+        memcpy( (void *) (ctx->buffer + left),
+                (void *) input, length );
+    }
+}
+
+static uint8 md5_padding[64] =
+{
+ 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+void md5_finish( md5_context *ctx, uint8 digest[16] )
+{
+    uint32 last, padn;
+    uint32 high, low;
+    uint8 msglen[8];
+
+    high = ( ctx->total[0] >> 29 )
+         | ( ctx->total[1] <<  3 );
+    low  = ( ctx->total[0] <<  3 );
+
+    PUT_UINT32( low,  msglen, 0 );
+    PUT_UINT32( high, msglen, 4 );
+
+    last = ctx->total[0] & 0x3F;
+    padn = ( last < 56 ) ? ( 56 - last ) : ( 120 - last );
+
+    md5_update( ctx, md5_padding, padn );
+    md5_update( ctx, msglen, 8 );
+
+    PUT_UINT32( ctx->state[0], digest,  0 );
+    PUT_UINT32( ctx->state[1], digest,  4 );
+    PUT_UINT32( ctx->state[2], digest,  8 );
+    PUT_UINT32( ctx->state[3], digest, 12 );
+}
+
diff --git a/utils/MTP/beastpatcher/md5.h b/utils/MTP/beastpatcher/md5.h
new file mode 100644
index 0000000000..71fa395548
--- /dev/null
+++ b/utils/MTP/beastpatcher/md5.h
@@ -0,0 +1,25 @@
+#ifndef _MD5_H
+#define _MD5_H
+
+#ifndef uint8
+#define uint8  unsigned char
+#endif
+
+#ifndef uint32
+#define uint32 unsigned long int
+#endif
+
+typedef struct
+{
+    uint32 total[2];
+    uint32 state[4];
+    uint8 buffer[64];
+}
+md5_context;
+
+void md5_starts( md5_context *ctx );
+void md5_update( md5_context *ctx, uint8 *input, uint32 length );
+void md5_finish( md5_context *ctx, uint8 digest[16] );
+
+#endif /* md5.h */
+
diff --git a/utils/MTP/beastpatcher/mknkboot.c b/utils/MTP/beastpatcher/mknkboot.c
index 9cc58d552c..9efc5d7595 100644
--- a/utils/MTP/beastpatcher/mknkboot.c
+++ b/utils/MTP/beastpatcher/mknkboot.c
@@ -42,6 +42,7 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
+#include "md5.h"
 #if defined(_MSC_VER)
 #include "pstdint.h"
 #else
@@ -99,6 +100,21 @@ mknkboot.c appends two images:
 #define DISABLE_INSN    0xe3a00001
 #define DISABLE_SUM     (0xe3+0xa0+0x00+0x01)  
 
+// firmware table struct
+struct firmentry
+{
+    int version;
+    uint8_t sum[16];
+};
+
+// firmware table
+static const struct firmentry firmtable[] =
+{
+    {0x0102, "\xdc\xd8\xe9\x04\x2c\x4d\x14\x84\x85\xbe\xef\x9c\xa5\xe6\x7b\x90"},
+    {0x0103, "\x19\x9f\x97\x5b\x5e\xef\x66\xf8\x91\x2c\x34\xf2\x11\x4c\xb1\xb4"},
+    {},
+};
+
 /* Code to dual-boot - this is inserted at NK_ENTRY_POINT */
 static uint32_t dualboot[] = 
 {
@@ -119,7 +135,6 @@ static uint32_t dualboot[] =
     BL_ENTRY_POINT   /* RB bootloader load address/entry point */
 };
 
-
 static void put_uint32le(uint32_t x, unsigned char* p)
 {
     p[0] = (unsigned char)(x & 0xff);
@@ -141,6 +156,29 @@ static off_t filesize(int fd) {
 }
 #endif
 
+int verifyfirm(const struct filebuf* firmdata)
+{
+    for(int i = 0; firmtable[i].version; i++)
+    {
+        md5_context ctx;
+	uint8_t sum[16];
+
+	md5_starts(&ctx);
+	md5_update(&ctx, firmdata->buf, firmdata->len);
+	md5_finish(&ctx, sum);
+
+	if(memcmp(firmtable[i].sum, sum, 16) == 0)
+        {
+            fprintf(stderr, "[INFO]  Firmware file version %d.%d\n",
+                    firmtable[i].version >> 8, firmtable[i].version & 0xff);
+            return firmtable[i].version;
+        }
+    }
+
+    fprintf(stderr, "[ERR]  Unknown firmware file!\n");
+
+    return -1;
+}
 
 int mknkboot(const struct filebuf *indata, const struct filebuf *bootdata,
              struct filebuf *outdata)
@@ -158,7 +196,7 @@ int mknkboot(const struct filebuf *indata, const struct filebuf *bootdata,
 
     if (outdata->buf==NULL)
     {
-        printf("[ERR]  Could not allocate memory, aborting\n");
+        fprintf(stderr, "[ERR]  Could not allocate memory, aborting\n");
         return -1;
     }
 
@@ -220,7 +258,7 @@ int mknkboot(const struct filebuf *indata, const struct filebuf *bootdata,
 #if !defined(BEASTPATCHER)
 static void usage(void)
 {
-    printf("Usage: mknkboot <firmware file> <boot file> <output file>\n");
+    fprintf(stderr, "Usage: mknkboot <firmware file> <boot file> <output file>\n");
 
     exit(1);
 }
@@ -264,25 +302,30 @@ int main(int argc, char* argv[])
     bootdata.buf = (unsigned char*)malloc(bootdata.len);
     if(indata.buf == NULL || bootdata.buf == NULL)
     {
-        printf("[ERR]  Could not allocate memory, aborting\n");
+        fprintf(stderr, "[ERR]  Could not allocate memory, aborting\n");
         result = 4;
         goto quit;
     }
     n = read(fdin, indata.buf, indata.len);
     if (n != indata.len)
     {
-        printf("[ERR]  Could not read from %s\n",infile);
+        fprintf(stderr, "[ERR]  Could not read from %s\n",infile);
         result = 5;
         goto quit;
     }
     n = read(fdboot, bootdata.buf, bootdata.len);
     if (n != bootdata.len)
     {
-        printf("[ERR]  Could not read from %s\n",bootfile);
+        fprintf(stderr, "[ERR]  Could not read from %s\n",bootfile);
         result = 6;
         goto quit;
     }
 
+    if (verifyfirm(&indata) < 0)
+    {
+        result = 7;
+        goto quit;
+    }
     result = mknkboot(&indata, &bootdata, &outdata);
     if(result != 0)
     {
@@ -292,15 +335,15 @@ int main(int argc, char* argv[])
     if (fdout < 0)
     {
         perror(outfile);
-        result = 7;
+        result = 8;
         goto quit;
     }
 
     n = write(fdout, outdata.buf, outdata.len);
     if (n != outdata.len)
     {
-        printf("[ERR] Could not write output file %s\n",outfile);
-        result = 8;
+        fprintf(stderr, "[ERR] Could not write output file %s\n",outfile);
+        result = 9;
         goto quit;
     }
 
diff --git a/utils/MTP/beastpatcher/mknkboot.h b/utils/MTP/beastpatcher/mknkboot.h
index 0373421a17..162cfc5ddf 100644
--- a/utils/MTP/beastpatcher/mknkboot.h
+++ b/utils/MTP/beastpatcher/mknkboot.h
@@ -43,6 +43,7 @@ struct filebuf {
     unsigned char* buf;
 };
 
+int verifyfirm(const struct filebuf* firmdata);
 int mknkboot(const struct filebuf *indata, const struct filebuf *bootdata,
              struct filebuf *outdata);
 #endif