diff --git a/include/mbedtls/config.h b/include/mbedtls/config.h
index e7f42e5..2e1b982 100644
--- a/include/mbedtls/config.h
+++ b/include/mbedtls/config.h
@@ -3365,7 +3365,8 @@
  * Note: A value of \c 1 means that no padding will be used
  *       for outgoing records.
  *
- * The value MUST be a power of 2.
+ * Note: On systems lacking division instructions,
+ *       a power of two should be preferred.
  *
  */
 //#define MBEDTLS_SSL_CID_PADDING_GRANULARITY 16
diff --git a/library/ssl_tls.c b/library/ssl_tls.c
index d16ce01..428bab7 100644
--- a/library/ssl_tls.c
+++ b/library/ssl_tls.c
@@ -2013,9 +2013,9 @@
                                           uint8_t rec_type )
 {
     size_t len = *content_size;
-
-    /* MBEDTLS_SSL_CID_PADDING_GRANULARITY must be a power of 2. */
-    size_t pad = ~len & ( MBEDTLS_SSL_CID_PADDING_GRANULARITY - 1 );
+    size_t pad = ( MBEDTLS_SSL_CID_PADDING_GRANULARITY -
+                   ( len + 1 ) % MBEDTLS_SSL_CID_PADDING_GRANULARITY ) %
+        MBEDTLS_SSL_CID_PADDING_GRANULARITY;
 
     /* Write real content type */
     if( remaining == 0 )