diff --git a/programs/pkey/mpi_demo.c b/programs/pkey/mpi_demo.c
index 3b7c085..7281c3a 100644
--- a/programs/pkey/mpi_demo.c
+++ b/programs/pkey/mpi_demo.c
@@ -48,55 +48,63 @@
 #else
 int main( void )
 {
+    int ret;
     mpi E, P, Q, N, H, D, X, Y, Z;
 
     mpi_init( &E ); mpi_init( &P ); mpi_init( &Q ); mpi_init( &N );
     mpi_init( &H ); mpi_init( &D ); mpi_init( &X ); mpi_init( &Y );
     mpi_init( &Z );
 
-    mpi_read_string( &P, 10, "2789" );
-    mpi_read_string( &Q, 10, "3203" );
-    mpi_read_string( &E, 10,  "257" );
-    mpi_mul_mpi( &N, &P, &Q );
+    MPI_CHK( mpi_read_string( &P, 10, "2789" ) );
+    MPI_CHK( mpi_read_string( &Q, 10, "3203" ) );
+    MPI_CHK( mpi_read_string( &E, 10,  "257" ) );
+    MPI_CHK( mpi_mul_mpi( &N, &P, &Q ) );
 
     polarssl_printf( "\n  Public key:\n\n" );
-    mpi_write_file( "  N = ", &N, 10, NULL );
-    mpi_write_file( "  E = ", &E, 10, NULL );
+    MPI_CHK( mpi_write_file( "  N = ", &N, 10, NULL ) );
+    MPI_CHK( mpi_write_file( "  E = ", &E, 10, NULL ) );
 
     polarssl_printf( "\n  Private key:\n\n" );
-    mpi_write_file( "  P = ", &P, 10, NULL );
-    mpi_write_file( "  Q = ", &Q, 10, NULL );
+    MPI_CHK( mpi_write_file( "  P = ", &P, 10, NULL ) );
+    MPI_CHK( mpi_write_file( "  Q = ", &Q, 10, NULL ) );
 
 #if defined(POLARSSL_GENPRIME)
-    mpi_sub_int( &P, &P, 1 );
-    mpi_sub_int( &Q, &Q, 1 );
-    mpi_mul_mpi( &H, &P, &Q );
-    mpi_inv_mod( &D, &E, &H );
+    MPI_CHK( mpi_sub_int( &P, &P, 1 ) );
+    MPI_CHK( mpi_sub_int( &Q, &Q, 1 ) );
+    MPI_CHK( mpi_mul_mpi( &H, &P, &Q ) );
+    MPI_CHK( mpi_inv_mod( &D, &E, &H ) );
 
     mpi_write_file( "  D = E^-1 mod (P-1)*(Q-1) = ",
                     &D, 10, NULL );
 #else
     polarssl_printf("\nTest skipped (POLARSSL_GENPRIME not defined).\n\n");
 #endif
-    mpi_read_string( &X, 10, "55555" );
-    mpi_exp_mod( &Y, &X, &E, &N, NULL );
-    mpi_exp_mod( &Z, &Y, &D, &N, NULL );
+    MPI_CHK( mpi_read_string( &X, 10, "55555" ) );
+    MPI_CHK( mpi_exp_mod( &Y, &X, &E, &N, NULL ) );
+    MPI_CHK( mpi_exp_mod( &Z, &Y, &D, &N, NULL ) );
 
     polarssl_printf( "\n  RSA operation:\n\n" );
-    mpi_write_file( "  X (plaintext)  = ", &X, 10, NULL );
-    mpi_write_file( "  Y (ciphertext) = X^E mod N = ", &Y, 10, NULL );
-    mpi_write_file( "  Z (decrypted)  = Y^D mod N = ", &Z, 10, NULL );
+    MPI_CHK( mpi_write_file( "  X (plaintext)  = ", &X, 10, NULL ) );
+    MPI_CHK( mpi_write_file( "  Y (ciphertext) = X^E mod N = ", &Y, 10, NULL ) );
+    MPI_CHK( mpi_write_file( "  Z (decrypted)  = Y^D mod N = ", &Z, 10, NULL ) );
     polarssl_printf( "\n" );
 
+cleanup:
     mpi_free( &E ); mpi_free( &P ); mpi_free( &Q ); mpi_free( &N );
     mpi_free( &H ); mpi_free( &D ); mpi_free( &X ); mpi_free( &Y );
     mpi_free( &Z );
 
+    if( ret != 0 )
+    {
+        polarssl_printf( "\nAn error occured.\n" );
+        ret = 1;
+    }
+
 #if defined(_WIN32)
     polarssl_printf( "  Press Enter to exit this program.\n" );
     fflush( stdout ); getchar();
 #endif
 
-    return( 0 );
+    return( ret );
 }
 #endif /* POLARSSL_BIGNUM_C && POLARSSL_FS_IO */