spintop.c

/*
 * spintop.c
 *
 *
 * Copyright (c) 2012 Gonzalo J. Carracedo
 * < BatchDrake (at) gmail (dot) com >
 *
 * SpinTop: implements bsign-style signature self-verification at runtime, thus
 * preventing infected / modified binaries to run.
 *
 * SpinTop 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.
 *
 * SpinTop 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 Slabot; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Note: SHA1 implementation comes directly from bsign's code.
 */

#define _FILE_OFFSET_BITS 64

#include <stdlib.h>
#include <elf.h>
#include <stdarg.h>
#include <fcntl.h>
#include <unistd.h>
#include <syscall.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <stdint.h>
#include <gpgme.h>
#include <stdint.h>

#include "standard.h"
#include "sha1.h"

#ifndef SPINTOP_TRUSTABLE_FINGERPRINT
  #error SPINTOP_TRUSTABLE_FINGERPRINT is not defined, please provide a valid signature fingerprint at compile time.
#endif

#if UINTPTR_MAX == 0xffffffff
  #define Elf_Ehdr Elf32_Ehdr
  #define Elf_Shdr Elf32_Shdr
#elif UINTPTR_MAX == 0xffffffffffffffff
  #define Elf_Ehdr Elf64_Ehdr
  #define Elf_Shdr Elf64_Shdr
#else
  #error Unsupported machine
#endif

#define warning(fmt, arg...) st_printf ("spintop warning: " fmt, ##arg)
#define error(fmt, arg...) st_printf ("\033[1;31mspintop error: " fmt "\033[0m", ##arg)


typedef int8_t byte;
typedef int8_t int8;
typedef uint8_t unsigned8;
typedef int16_t int16;
typedef uint16_t unsigned16;

typedef int32_t int32;
typedef uint32_t unsigned32;

typedef int64_t int64;
typedef uint64_t unsigned64;


/* SHA1 functions */
const char *
sha1_get_info( int algo, size_t *contextsize,
           byte **r_asnoid, int *r_asn_len, int *r_mdlen,
           void (**r_init)( void *c ),
           void (**r_write)( void *c, byte *buf, size_t nbytes ),
           void (**r_final)( void *c ),
           byte *(**r_read)( void *c )
         );

/* System calls */
static inline int     st_open (const char *, int);
static inline int     st_close (int);
static inline int     st_write (int, const void *, int);
static inline int     st_read (int, void *, int);
static inline int     st_lseek (int, int, int);
static inline pid_t   st_getpid (void);
static inline int     st_exit (int code);
void *                st_mmap (void *, size_t, int, int, int, off_t) __attribute__ ((visibility ("hidden")));
int                   st_munmap (void *, size_t) __attribute__ ((visibility ("hidden")));


/* Library functions */
static size_t         st_vsnprintf (char *, size_t, const char *, va_list);
static void           st_printf (const char *, ...);
static void           st_snprintf (char *, size_t, const char *, ...);
static inline size_t  st_strlen (const char *);
static inline int     st_memset (void *, int, size_t);
static inline int     st_strcmp (const char *, const char *);
static inline void *  st_memcpy (void *, const void *, size_t);

/*
 Definitions end here
 -----8<-----------------------------------------------------------------------
 Spintop code starts here
*/

/* Just a function to return a hash of the binary */
char *
sha1_hash_binary (void *elfbase, size_t elflength)
{
  size_t ctx_size;
  void (*pfn_init) (void*);
  void (*pfn_write) (void*, byte*, size_t);
  void (*pfn_final) (void*);
  byte* (*pfn_read) (void*);
  byte* r_asnoid;
  int r_asnlen;
  int r_mdlen;
  void *ctx;

  static char result[20];

  sha1_get_info (2, &ctx_size, &r_asnoid, &r_asnlen, &r_mdlen,
    &pfn_init, &pfn_write, &pfn_final, &pfn_read);

  if ((ctx = malloc (ctx_size)) == NULL)
  {
    warning ("cannot allocate %d bytes, exiting\n", ctx_size);
    st_exit (127);
  }

  (*pfn_init) (ctx);
  (*pfn_write) (ctx, elfbase, elflength);
  (*pfn_final) (ctx);

  st_memcpy (result, (*pfn_read) (ctx), 20);

  free (ctx);

  return result;
}

/* Verify whether GPG signature works */
static int
gpg_signature_verify (const void *signature_data, size_t signature_length, void *hash_data, size_t hash_len)
{
  gpgme_ctx_t ctx;
  gpgme_error_t err;
  gpgme_data_t sig_buf, hash_buf;
  gpgme_verify_result_t result;
  int errcode;

  /* It's REALLY IMPORTANT that the fingerprint is stored as a local variable
     instead as a constant written somewhere in .rodata. Like this, the
     fingerprint will be initialized with a series of mov's, splitting
     it, making it more difficult to find from a virus perspective. */
  char expected_fingerprint[] = SPINTOP_TRUSTABLE_FINGERPRINT;

  (void) gpgme_check_version (NULL);

  errcode = -1;

  if ((err = gpgme_new (&ctx)) != 0)
  {
    warning ("cannot create GPGME context, cannot continue.\n");

    return -1;
  }

  if ((err = gpgme_set_protocol (ctx, GPGME_PROTOCOL_OpenPGP)) != 0)
  {
    warning ("cannot select OpenPGP as protocol, cannot continue.\n");

    goto ctx_cleanup;
  }

  /* Checking a valid message.  */
  if ((err = gpgme_data_new_from_mem (&sig_buf, signature_data, signature_length, 0)) != 0)
  {
    warning ("cannot copy signature, cannot continue.\n");

    goto ctx_cleanup;
  }

  if ((err = gpgme_data_new_from_mem (&hash_buf, hash_data, hash_len, 0)) != 0)
  {
    warning ("cannot copy hash, cannot continue.\n");

    goto sig_cleanup;
  }

  if ((err = gpgme_op_verify (ctx, sig_buf, hash_buf, NULL)) != 0)
  {
    error ("verification error (%s), operation failed, cannot continue.\n", gpgme_strerror (err));
    goto all_cleanup;
  }

  result = gpgme_op_verify_result (ctx);

  if (gpg_err_code (result->signatures->status) != GPG_ERR_NO_ERROR)
  {
    error ("MODIFIED BINARY DETECTED (%s)\n", gpg_strerror (gpg_err_code (result->signatures->status)));
    error ("SOMEBODY CALL DE COPS\n");
    goto all_cleanup;
  }

  if (result->signatures->wrong_key_usage)
  {
    warning ("wrong key usage!\n");

    goto all_cleanup;
  }

  if (result->signatures->validity != GPGME_VALIDITY_FULL)
  {
    warning ("signature is not fully valid, cannot continue.\n");

    goto all_cleanup;
  }

  if (gpg_err_code (result->signatures->validity_reason) != GPG_ERR_NO_ERROR)
  {
    warning ("validity encountered errors, cannot continue.");
    goto all_cleanup;
  }

  if (st_strcmp (result->signatures->fpr, expected_fingerprint) != 0)
  {
    error ("WARNING: UNEXPECTED FINGERPRINT \"%s\"\n", result->signatures->fpr);
    error ("SOMEBODY TRIED TO FOOL US.\n");

    goto all_cleanup;
  }

  errcode = 0;

all_cleanup:
  gpgme_data_release (hash_buf);

sig_cleanup:
  gpgme_data_release (sig_buf);

ctx_cleanup:
  gpgme_release (ctx);

  return errcode;
}

/* Find a section inside an ELF */
static const Elf_Shdr *
elf_section_lookup (const void *base, size_t length, const char *name)
{
  const Elf_Ehdr *header;
  const Elf_Shdr *shdr;
  const char *section_names;
  unsigned long names_len;

  int i;

  header = (Elf_Ehdr *) base;

  if (header->e_shoff + header->e_shnum * header->e_shentsize > length)
    return NULL;

  shdr = (Elf_Shdr *) (base +
                       header->e_shoff +
                       header->e_shstrndx * header->e_shentsize);

  if (shdr->sh_offset >= length)
    return NULL;

  section_names = (const char *) (base + shdr->sh_offset);
  names_len = shdr->sh_size;

  for (i = 0; i < header->e_shnum; ++i)
  {
    shdr = (Elf_Shdr *) (base + header->e_shoff + i * header->e_shentsize);

    if (shdr->sh_name > names_len)
      continue;

    if (st_strcmp (name, shdr->sh_name + section_names) == 0)
      return shdr;
  }

  return NULL;
}

/* Check if this ELF is ready to run */
static int
elf_selfcheck (void *base, size_t length)
{
  const Elf_Ehdr *header;
  const Elf_Shdr *signshdr;
  const void *sign_base;
  const char *str_sign_base;
  char *new_hash;
  size_t new_hash_len;

  char *signature_copy;
  uint16_t sigsize;

  int i;

  header = (Elf_Ehdr *) base;

  if (header->e_ident[EI_MAG0] != ELFMAG0 ||
      header->e_ident[EI_MAG1] != ELFMAG1 ||
      header->e_ident[EI_MAG2] != ELFMAG2 ||
      header->e_ident[EI_MAG3] != ELFMAG3)
  {
    warning ("not an ELF file!\n");
    return -1;
  }

  if ((signshdr = elf_section_lookup (base, length, "signature")) == NULL)
  {
    warning ("binary is not signed (yet). Use bsign(1) to generate a signature before running it.\n");
    return -1;
  }

  if (signshdr->sh_offset + signshdr->sh_size > length)
  {
    warning ("signature bytes out of bounds!\n");
    return -1;
  }

  sign_base = base + signshdr->sh_offset;
  str_sign_base = (const char *) sign_base;

  if (str_sign_base[0] != '#' ||
      str_sign_base[1] != '1' ||
      str_sign_base[2] != ';')
  {
    warning ("unrecognized signature format!\n");
    return -1;
  }


  for (i = 3; i < signshdr->sh_size; ++i)
    if (str_sign_base[i] == '\n')
      break;

  if (str_sign_base[i++] != '\n')
  {
    warning ("cannot find signature start!\n");
    return -1;
  }

  i += 20;

  new_hash_len = i;

  if ((new_hash = malloc (new_hash_len)) == NULL)
  {
    warning ("malloc: cannot alloc %d bytes\n", i);
    return -1;
  }

  st_memcpy (new_hash, sign_base, new_hash_len);

  sigsize = 0;

  sigsize |= (unsigned) str_sign_base[i++] << 8;
  sigsize |= (unsigned) str_sign_base[i++];

  if ((signature_copy = malloc (sigsize)) == NULL)
  {
    warning ("malloc: cannot alloc %d bytes\n", i);
    return -1;
  }

  st_memcpy (signature_copy, str_sign_base + i, sigsize);

  st_memset (base + signshdr->sh_offset, 0, signshdr->sh_size);

  st_memcpy (new_hash + new_hash_len - 20, sha1_hash_binary (base, length), 20);

  if (gpg_signature_verify (signature_copy, sigsize, new_hash, new_hash_len) == -1)
  {
    free (new_hash);
    free (signature_copy);
    return -1;
  }

  free (new_hash);
  free (signature_copy);

  return 0;
}

/* Entry point */
__attribute__ ((constructor)) static void
spintop_entry (void)
{
  char procpath[100];
  int fd, ret;
  ssize_t bin_length;
  void *base;

  st_snprintf (procpath, sizeof (procpath), "/proc/%d/exe", st_getpid ());

  if ((fd = st_open (procpath, O_RDONLY)) < 0)
  {
    warning ("cannot open %s for reading: errno %d\n", procpath, -fd);
    st_exit (127);
  }

  if ((bin_length = st_lseek (fd, 0, SEEK_END)) < 0)
  {
    warning ("cannot self-measure executable size: errno %d\n", -bin_length);
    st_exit (127);
  }

  if ((unsigned) (base = st_mmap (NULL, bin_length, PROT_READ | PROT_WRITE, MAP_PRIVATE, fd, 0)) & 0xfff)
  {
    warning ("mmap failed\n");
    st_exit (127);
  }

  if (elf_selfcheck (base, bin_length) == -1)
    st_exit (127);

  st_close (fd);

  if ((ret = st_munmap (base, bin_length)) < 0)
  {
    warning ("cannot munmap!\n");
    st_exit (127);
  }
}



/*
 Spintop code ends here
 -----8<-----------------------------------------------------------------------
 Library functions start here
*/

#define ST_PRINTF_BUFSIZE 256

/* Directly copied from Wikipedia. Yes, so what? I don't need a super-efficient
   implementation of this */
static inline int
st_strcmp (const char *s1, const char *s2)
{
   unsigned char uc1, uc2;
   /* Move s1 and s2 to the first differing characters
      in each string, or the ends of the strings if they
      are identical.  */
   while (*s1 != '\0' && *s1 == *s2) {
       s1++;
       s2++;
   }
   /* Compare the characters as unsigned char and
      return the difference.  */
   uc1 = (*(unsigned char *) s1);
   uc2 = (*(unsigned char *) s2);
   return ((uc1 < uc2) ? -1 : (uc1 > uc2));
}

static inline int
st_memset (void *dest, int code, size_t len)
{
  int olen = len;
  while (len--)
    *((char *) dest++) = code;

  return olen;
}

static inline void *
st_memcpy (void *dest, const void *orig, size_t len)
{
  while (len--)
    *((char *) dest++) = *((char *) orig++);

  return dest;
}

static void
st_printf (const char *fmt, ...)
{
  va_list ap;
  char buffer[ST_PRINTF_BUFSIZE];
  size_t len;

  va_start (ap, fmt);

  len = st_vsnprintf (buffer, ST_PRINTF_BUFSIZE, fmt, ap);

  st_write (2, buffer, len);

  va_end (ap);
}

static void
st_snprintf (char *buffer, size_t len, const char *fmt, ...)
{
  va_list ap;
  va_start (ap, fmt);

  len = st_vsnprintf (buffer, ST_PRINTF_BUFSIZE, fmt, ap);

  va_end (ap);
}

#define APPEND_CHAR(output, size, c) \
  do                                 \
  {                                  \
    if (*size)                       \
    {                                \
      *output++ = c;                 \
      --(*size);                     \
    }                                \
    else                             \
      return output;                 \
  }                                  \
  while (0)

static char *
convert_decimal (char *output, size_t *size, int n)
{
  int copy;
  int i;
  char nbuf[30];

  copy = n;
  if (copy < 0)
    copy = -copy;

  for (i = 0; copy; ++i)
  {
    nbuf[i] = '0' + copy % 10;
    copy /= 10;
  }

  if (n < 0)
    APPEND_CHAR (output, size, '-');

  if (!i)
    APPEND_CHAR (output, size, '0');
  else
    for (--i; i >= 0; --i)
      APPEND_CHAR (output, size, nbuf[i]);

  return output;
}


static char *
convert_octal (char *output, size_t *size, unsigned int n)
{
  int copy;
  int i;
  char nbuf[30];

  copy = n;
  if (copy < 0)
    copy = -copy;

  for (i = 0; copy; ++i)
  {
    nbuf[i] = '0' + copy % 8;
    copy /= 8;
  }

  if (!i)
    APPEND_CHAR (output, size, '0');
  else
    for (--i; i >= 0; --i)
      APPEND_CHAR (output, size, nbuf[i]);

  return output;
}

static char *
convert_hex64 (char *output, size_t *size, uint64_t n, int upper)
{
  int copy;
  int i;
  char nbuf[30];
  char hexachars_upper[] = "0123456789ABCDEF";
  char hexachars_lower[] = "0123456789abcdef";

  for (i = 0; n; ++i)
  {
    nbuf[i] = upper ? hexachars_upper [n % 16] : hexachars_lower [n % 16];
    n /= 16;
  }

  if (!i)
    APPEND_CHAR (output, size, '0');
  else
    for (--i; i >= 0; --i)
      APPEND_CHAR (output, size, nbuf[i]);

  return output;
}

static char *
convert_DWORD (char *output, size_t *size, uint32_t num, int upper, int sep)
{
  char hexachars_upper[] = "0123456789ABCDEF";
  char hexachars_lower[] = "0123456789abcdef";

  int i;

  for (i = 7; i >= 0; --i)
  {
    if (i == 3 && sep)
      APPEND_CHAR (output, size, ':');

    APPEND_CHAR (output, size, upper ? hexachars_upper[(num & 0xf << i * 4) >> i * 4] :
              hexachars_lower[(num & 0xf << i * 4) >> i * 4]);
  }

  return output;
}

static char *
convert_QWORD (char *output, size_t *size, uint64_t num, int upper, int sep)
{
  char hexachars_upper[] = "0123456789ABCDEF";
  char hexachars_lower[] = "0123456789abcdef";

  int i;

  for (i = 15; i >= 0; --i)
  {
    if (i == 7 && sep)
      APPEND_CHAR (output, size, ':');

    APPEND_CHAR (output, size, upper ? hexachars_upper[(num & 0xf << i * 4) >> i * 4] :
              hexachars_lower[(num & 0xf << i * 4) >> i * 4]);
  }

  return output;
}

static char *
convert_byte (char *output, size_t *size, uint32_t num, int upper)
{
  char hexachars_upper[] = "0123456789ABCDEF";
  char hexachars_lower[] = "0123456789abcdef";

  APPEND_CHAR (output, size, upper ? hexachars_upper[(num & 0xf << 4) >> 4] :
              hexachars_lower[(num & 0xf << 4) >> 4]);

  APPEND_CHAR (output, size, upper ? hexachars_upper[(num & 0xf)] :
              hexachars_lower[(num & 0xf)]);

  return output;
}

static char *
st_vsnprintf_append_char (char *output, size_t *size, char c)
{
  APPEND_CHAR (output, size, c);

  return output;
}

static char *
st_vsnprintf_append_string (char *output, size_t *size, const char *s)
{
  while (*s)
    APPEND_CHAR (output, size, *s++);

  return output;
}

static size_t
st_vsnprintf (char *output, size_t size, const char *fmt, va_list ap)
{

  int len;
  int i;
  void *ptr;
  size_t old;

  len = st_strlen (fmt);

  i = 0;

  old = size;

  --size;

  for (i = 0; i < len; ++i)
  {
    if (size <= 0)
      break;

    if (fmt[i] == '%')
    {
      ++i;

      switch (fmt[i])
      {
        case 'd':
          output = convert_decimal (output, &size, va_arg (ap, int));
          break;

        case 'b':
          output = convert_byte (output, &size, va_arg (ap, unsigned int), 0);
          break;

        case 'B':
          output = convert_byte (output, &size, va_arg (ap, unsigned int), 1);
          break;

        case 'x':
          output = convert_hex64 (output, &size, va_arg (ap, unsigned int), 0);
          break;

        case 'o':
          output = convert_octal (output, &size, va_arg (ap, unsigned int));
          break;

        case 'X':
          output = convert_hex64 (output, &size, va_arg (ap, unsigned int), 1);
          break;

        case 'w':
          output = convert_DWORD (output, &size, va_arg (ap, uint32_t), 0, 1);
          break;

        case 'W':
          output = convert_DWORD (output, &size, va_arg (ap, uint32_t), 1, 1);
          break;

        case 'q':
          output = convert_QWORD (output, &size, va_arg (ap, uint32_t), 0, 1);
          break;

        case 'Q':
          output = convert_QWORD (output, &size, va_arg (ap, uint32_t), 1, 1);
          break;

        case 'y':
          output = convert_DWORD (output, &size, va_arg (ap, uint32_t), 0, 0);
          break;

        case 'Y':
          output = convert_DWORD (output, &size, va_arg (ap, uint32_t), 1, 0);
          break;

        case 'z':
          output = convert_DWORD (output, &size, va_arg (ap, uint32_t), 0, 0);
          break;

        case 'Z':
          output = convert_DWORD (output, &size, va_arg (ap, uint32_t), 1, 0);
          break;

        case 'c':
          output = st_vsnprintf_append_char (output, &size, va_arg (ap, unsigned int));
          break;

        case 'p':
          ptr = va_arg (ap, void *);

          if (ptr == NULL)
            output = st_vsnprintf_append_string (output, &size, "(null)");
          else
          {
            output = st_vsnprintf_append_string (output, &size, "0x");
            output = convert_hex64 (output, &size, (unsigned long) ptr, 0);
          }

          break;

        case 's':
          output = st_vsnprintf_append_string (output, &size, va_arg (ap, char *));
          break;

        case '%':
          output = st_vsnprintf_append_char (output, &size, '%');
          break;

        case '\0':
          return;
      }
    }
    else
      output = st_vsnprintf_append_char (output, &size, fmt[i]);
  }

  *output = 0;

  return old - size - 1;
}


static inline size_t
st_strlen (const char *string)
{
  size_t size = 0;
  /* We aren't GNU hackers, the classical way works */

  while (*string++)
    ++size;

  return size;
}

/*
 Library functions end here
 -----8<-----------------------------------------------------------------------
 System calls start here
*/


static inline pid_t
st_getpid (void)
{
  pid_t result;

  asm volatile ("int $0x80" : "=a" (result) : "a" (__NR_getpid));

  return result;
}

static inline int
st_write (int fd, const void *buf, int size)
{
  int ret;

  asm volatile ("xchg %%ebx, %%esi\n"
                 "int $0x80\n"
                 "xchg %%ebx, %%esi\n" : "=a" (ret) :
                 "a" (__NR_write), "S" (fd), "c" (buf), "d" (size));

  return ret;
}

static inline int
st_read (int fd, void *buf, int size)
{
  int ret;

  asm volatile ("xchg %%ebx, %%esi\n"
                 "int $0x80\n"
                 "xchg %%ebx, %%esi\n" : "=a" (ret) :
                 "a" (__NR_read), "S" (fd), "c" (buf), "d" (size)  :
                 "memory"); /* read modifica la memoria */

  return ret;
}

static inline int
st_lseek (int fd, int offset, int whence)
{
  int ret;

  asm volatile ("xchg %%ebx, %%esi\n"
                 "int $0x80\n"
                 "xchg %%ebx, %%esi\n" : "=a" (ret) :
                 "a" (__NR_lseek), "S" (fd), "c" (offset), "d" (whence));

  return ret;
}

static inline int
st_open (const char *path, int mode)
{
  int ret;

  asm volatile ("xchg %%ebx, %%esi\n"
                 "int $0x80\n"
                 "xchg %%ebx, %%esi\n" : "=a" (ret) :
                 "a" (__NR_open), "S" (path), "c" (mode));

  return ret;
}

static inline int
st_close (int fd)
{
  int ret;

  asm volatile ("xchg %%ebx, %%esi\n"
                 "int $0x80\n"
                 "xchg %%ebx, %%esi\n" : "=a" (ret) :
                 "a" (__NR_close), "S" (fd));

  return ret;
}

static inline int
st_exit (int code)
{
  asm volatile ("xchg %%ebx, %%esi\n"
                 "int $0x80\n"
                 "xchg %%ebx, %%esi\n" : :
                 "a" (__NR_exit), "S" (code));
}

asm (".section .text");
asm ("st_munmap:");
asm ("  mov    %ebx,%edx");
asm ("  mov    0x8(%esp),%ecx");
asm ("  mov    0x4(%esp),%ebx");
asm ("  mov    $0x5b,%eax");
asm ("  int    $0x80");
asm ("  mov    %edx,%ebx");
asm ("  ret");

asm ("st_mmap:");
asm ("  push %eax");
asm ("  pusha");
asm ("  mov    0x28(%esp), %ebx"); /* Primer argumento */
asm ("  mov    0x2c(%esp), %ecx"); /* Segundo argumento */
asm ("  mov    0x30(%esp), %edx"); /* Tercer argumento */
asm ("  mov    0x34(%esp), %esi"); /* Cuargo argumento */
asm ("  mov    0x38(%esp), %edi"); /* Quinto argumnto */
asm ("  mov    0x3c(%esp), %ebp"); /* Sexto argumento */
asm ("  shr    $0xc, %ebp"); /* El kernel se espera directamente los 12 bits de página */
asm ("  mov    $0xc0, %eax");
asm ("  int    $0x80");
asm ("  mov    %eax, 0x20(%esp)");
asm ("  popa");
asm ("  pop %eax");
asm ("  ret");

/*
 System calls end here
 -----8<-----------------------------------------------------------------------
 SHA1 implementation starts here
*/

#define u32 unsigned32
#define DIM(v) (sizeof(v)/sizeof((v)[0]))
#define DIMof(type,member)   DIM(((type *)0)->member)

typedef struct {
    u32  h0,h1,h2,h3,h4;
    u32  nblocks;
    byte buf[64];
    int  count;
} SHA1_CONTEXT;


#if defined(__GNUC__) && defined(__i386__)
static inline u32
rol(int n, u32 x)
{
    __asm__("roll %%cl,%0"
        :"=r" (x)
        :"0" (x),"c" (n));
    return x;
}
#else
  #define rol(n,x)  ( ((x) << (n)) | ((x) >> (32-(n))) )
#endif




void
sha1_init( SHA1_CONTEXT *hd )
{
    hd->h0 = 0x67452301;
    hd->h1 = 0xefcdab89;
    hd->h2 = 0x98badcfe;
    hd->h3 = 0x10325476;
    hd->h4 = 0xc3d2e1f0;
    hd->nblocks = 0;
    hd->count = 0;
}


/****************
 * Transform the message X which consists of 16 32-bit-words
 */
static void
transform( SHA1_CONTEXT *hd, byte *data )
{
    u32 a,b,c,d,e,tm;
    u32 x[16];

    /* get values from the chaining vars */
    a = hd->h0;
    b = hd->h1;
    c = hd->h2;
    d = hd->h3;
    e = hd->h4;

#ifdef WORDS_BIGENDIAN
    memcpy( x, data, 64 );
#else
    { int i;
      byte *p2;
      for(i=0, p2=(byte*)x; i < 16; i++, p2 += 4 ) {
    p2[3] = *data++;
    p2[2] = *data++;
    p2[1] = *data++;
    p2[0] = *data++;
      }
    }
#endif


#define K1  0x5A827999L
#define K2  0x6ED9EBA1L
#define K3  0x8F1BBCDCL
#define K4  0xCA62C1D6L
#define F1(x,y,z)   ( z ^ ( x & ( y ^ z ) ) )
#define F2(x,y,z)   ( x ^ y ^ z )
#define F3(x,y,z)   ( ( x & y ) | ( z & ( x | y ) ) )
#define F4(x,y,z)   ( x ^ y ^ z )


#define M(i) ( tm =   x[i&0x0f] ^ x[(i-14)&0x0f] \
            ^ x[(i-8)&0x0f] ^ x[(i-3)&0x0f] \
           , (x[i&0x0f] = (tm << 1) | (tm >> 31)) )

#define R(a,b,c,d,e,f,k,m)  do { e += rol( 5, a )     \
                      + f( b, c, d )  \
                      + k         \
                      + m;        \
                 b = rol( 30, b );    \
                   } while(0)
    R( a, b, c, d, e, F1, K1, x[ 0] );
    R( e, a, b, c, d, F1, K1, x[ 1] );
    R( d, e, a, b, c, F1, K1, x[ 2] );
    R( c, d, e, a, b, F1, K1, x[ 3] );
    R( b, c, d, e, a, F1, K1, x[ 4] );
    R( a, b, c, d, e, F1, K1, x[ 5] );
    R( e, a, b, c, d, F1, K1, x[ 6] );
    R( d, e, a, b, c, F1, K1, x[ 7] );
    R( c, d, e, a, b, F1, K1, x[ 8] );
    R( b, c, d, e, a, F1, K1, x[ 9] );
    R( a, b, c, d, e, F1, K1, x[10] );
    R( e, a, b, c, d, F1, K1, x[11] );
    R( d, e, a, b, c, F1, K1, x[12] );
    R( c, d, e, a, b, F1, K1, x[13] );
    R( b, c, d, e, a, F1, K1, x[14] );
    R( a, b, c, d, e, F1, K1, x[15] );
    R( e, a, b, c, d, F1, K1, M(16) );
    R( d, e, a, b, c, F1, K1, M(17) );
    R( c, d, e, a, b, F1, K1, M(18) );
    R( b, c, d, e, a, F1, K1, M(19) );
    R( a, b, c, d, e, F2, K2, M(20) );
    R( e, a, b, c, d, F2, K2, M(21) );
    R( d, e, a, b, c, F2, K2, M(22) );
    R( c, d, e, a, b, F2, K2, M(23) );
    R( b, c, d, e, a, F2, K2, M(24) );
    R( a, b, c, d, e, F2, K2, M(25) );
    R( e, a, b, c, d, F2, K2, M(26) );
    R( d, e, a, b, c, F2, K2, M(27) );
    R( c, d, e, a, b, F2, K2, M(28) );
    R( b, c, d, e, a, F2, K2, M(29) );
    R( a, b, c, d, e, F2, K2, M(30) );
    R( e, a, b, c, d, F2, K2, M(31) );
    R( d, e, a, b, c, F2, K2, M(32) );
    R( c, d, e, a, b, F2, K2, M(33) );
    R( b, c, d, e, a, F2, K2, M(34) );
    R( a, b, c, d, e, F2, K2, M(35) );
    R( e, a, b, c, d, F2, K2, M(36) );
    R( d, e, a, b, c, F2, K2, M(37) );
    R( c, d, e, a, b, F2, K2, M(38) );
    R( b, c, d, e, a, F2, K2, M(39) );
    R( a, b, c, d, e, F3, K3, M(40) );
    R( e, a, b, c, d, F3, K3, M(41) );
    R( d, e, a, b, c, F3, K3, M(42) );
    R( c, d, e, a, b, F3, K3, M(43) );
    R( b, c, d, e, a, F3, K3, M(44) );
    R( a, b, c, d, e, F3, K3, M(45) );
    R( e, a, b, c, d, F3, K3, M(46) );
    R( d, e, a, b, c, F3, K3, M(47) );
    R( c, d, e, a, b, F3, K3, M(48) );
    R( b, c, d, e, a, F3, K3, M(49) );
    R( a, b, c, d, e, F3, K3, M(50) );
    R( e, a, b, c, d, F3, K3, M(51) );
    R( d, e, a, b, c, F3, K3, M(52) );
    R( c, d, e, a, b, F3, K3, M(53) );
    R( b, c, d, e, a, F3, K3, M(54) );
    R( a, b, c, d, e, F3, K3, M(55) );
    R( e, a, b, c, d, F3, K3, M(56) );
    R( d, e, a, b, c, F3, K3, M(57) );
    R( c, d, e, a, b, F3, K3, M(58) );
    R( b, c, d, e, a, F3, K3, M(59) );
    R( a, b, c, d, e, F4, K4, M(60) );
    R( e, a, b, c, d, F4, K4, M(61) );
    R( d, e, a, b, c, F4, K4, M(62) );
    R( c, d, e, a, b, F4, K4, M(63) );
    R( b, c, d, e, a, F4, K4, M(64) );
    R( a, b, c, d, e, F4, K4, M(65) );
    R( e, a, b, c, d, F4, K4, M(66) );
    R( d, e, a, b, c, F4, K4, M(67) );
    R( c, d, e, a, b, F4, K4, M(68) );
    R( b, c, d, e, a, F4, K4, M(69) );
    R( a, b, c, d, e, F4, K4, M(70) );
    R( e, a, b, c, d, F4, K4, M(71) );
    R( d, e, a, b, c, F4, K4, M(72) );
    R( c, d, e, a, b, F4, K4, M(73) );
    R( b, c, d, e, a, F4, K4, M(74) );
    R( a, b, c, d, e, F4, K4, M(75) );
    R( e, a, b, c, d, F4, K4, M(76) );
    R( d, e, a, b, c, F4, K4, M(77) );
    R( c, d, e, a, b, F4, K4, M(78) );
    R( b, c, d, e, a, F4, K4, M(79) );

    /* update chainig vars */
    hd->h0 += a;
    hd->h1 += b;
    hd->h2 += c;
    hd->h3 += d;
    hd->h4 += e;
}


/* Update the message digest with the contents
 * of INBUF with length INLEN.
 */
static void
sha1_write( SHA1_CONTEXT *hd, byte *inbuf, size_t inlen)
{
    if( hd->count == 64 ) { /* flush the buffer */
    transform( hd, hd->buf );
    hd->count = 0;
    hd->nblocks++;
    }
    if( !inbuf )
    return;
    if( hd->count ) {
    for( ; inlen && hd->count < 64; inlen-- )
        hd->buf[hd->count++] = *inbuf++;
    sha1_write( hd, NULL, 0 );
    if( !inlen )
        return;
    }

    while( inlen >= 64 ) {
    transform( hd, inbuf );
    hd->count = 0;
    hd->nblocks++;
    inlen -= 64;
    inbuf += 64;
    }
    for( ; inlen && hd->count < 64; inlen-- )
    hd->buf[hd->count++] = *inbuf++;
}


/* The routine final terminates the computation and
 * returns the digest.
 * The handle is prepared for a new cycle, but adding bytes to the
 * handle will the destroy the returned buffer.
 * Returns: 20 bytes representing the digest.
 */

static void
sha1_final(SHA1_CONTEXT *hd)
{
    u32 t, msb, lsb;
    byte *p;

    sha1_write(hd, NULL, 0); /* flush */;

    msb = 0;
    t = hd->nblocks;
    if( (lsb = t << 6) < t ) /* multiply by 64 to make a byte count */
    msb++;
    msb += t >> 26;
    t = lsb;
    if( (lsb = t + hd->count) < t ) /* add the count */
    msb++;
    t = lsb;
    if( (lsb = t << 3) < t ) /* multiply by 8 to make a bit count */
    msb++;
    msb += t >> 29;

    if( hd->count < 56 ) { /* enough room */
    hd->buf[hd->count++] = 0x80; /* pad */
    while( hd->count < 56 )
        hd->buf[hd->count++] = 0;  /* pad */
    }
    else { /* need one extra block */
    hd->buf[hd->count++] = 0x80; /* pad character */
    while( hd->count < 64 )
        hd->buf[hd->count++] = 0;
    sha1_write(hd, NULL, 0);  /* flush */;
    memset(hd->buf, 0, 56 ); /* fill next block with zeroes */
    }
    /* append the 64 bit count */
    hd->buf[56] = msb >> 24;
    hd->buf[57] = msb >> 16;
    hd->buf[58] = msb >>  8;
    hd->buf[59] = msb      ;
    hd->buf[60] = lsb >> 24;
    hd->buf[61] = lsb >> 16;
    hd->buf[62] = lsb >>  8;
    hd->buf[63] = lsb      ;
    transform( hd, hd->buf );

    p = hd->buf;
#ifdef WORDS_BIGENDIAN
# define X(a) do { *(u32*)p = hd->h##a ; p += 4; } while(0)
#else /* little endian */
# define X(a) do { *p++ = hd->h##a >> 24; *p++ = hd->h##a >> 16;     \
                   *p++ = hd->h##a >> 8; *p++ = hd->h##a; } while(0)
#endif
    X(0);
    X(1);
    X(2);
    X(3);
    X(4);
  #undef X

}

static byte *
sha1_read( SHA1_CONTEXT *hd )
{
    return hd->buf;
}

/****************
 * Return some information about the algorithm.  We need algo here to
 * distinguish different flavors of the algorithm.
 * Returns: A pointer to string describing the algorithm or NULL if
 *      the ALGO is invalid.
 */
const char *
sha1_get_info( int algo, size_t *contextsize,
           byte **r_asnoid, int *r_asnlen, int *r_mdlen,
           void (**r_init)( void *c ),
           void (**r_write)( void *c, byte *buf, size_t nbytes ),
           void (**r_final)( void *c ),
           byte *(**r_read)( void *c )
         )
{
    static byte asn[15] = /* Object ID is 1.3.14.3.2.26 */
            { 0x30, 0x21, 0x30, 0x09, 0x06, 0x05, 0x2b, 0x0e, 0x03,
              0x02, 0x1a, 0x05, 0x00, 0x04, 0x14 };
    if( algo != 2 )
    return NULL;

    *contextsize = sizeof(SHA1_CONTEXT);
    *r_asnoid = asn;
    *r_asnlen = DIM(asn);
    *r_mdlen = 20;
    *r_init  = (void (*)(void *))sha1_init;
    *r_write = (void (*)(void *, byte*, size_t))sha1_write;
    *r_final = (void (*)(void *))sha1_final;
    *r_read  = (byte *(*)(void *))sha1_read;

    return "SHA1";
}

/*
 SHA1 implementation ends here
 -----8<-----------------------------------------------------------------------
*/