135 lines
3.7 KiB
C
135 lines
3.7 KiB
C
/*
|
|
Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
|
|
Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
|
|
Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)
|
|
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU Lesser General Public
|
|
License as published by the Free Software Foundation; either
|
|
version 2.1 of the License, or (at your option) any later version.
|
|
|
|
This library 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
|
|
Lesser General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public
|
|
License along with this library; if not, write to the Free Software
|
|
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*/
|
|
|
|
/* Simple implementation of XTEA in CTR mode */
|
|
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
/* CSP includes */
|
|
#include <csp/csp.h>
|
|
#include <csp/csp_endian.h>
|
|
#include <csp/crypto/csp_sha1.h>
|
|
#include <csp/crypto/csp_xtea.h>
|
|
|
|
#ifdef CSP_USE_XTEA
|
|
|
|
#define XTEA_BLOCKSIZE 8
|
|
#define XTEA_ROUNDS 32
|
|
#define XTEA_KEY_LENGTH 16
|
|
|
|
/* XTEA key */
|
|
static uint32_t csp_xtea_key[XTEA_KEY_LENGTH/sizeof(uint32_t)] __attribute__ ((aligned(sizeof(uint32_t))));
|
|
|
|
#define STORE32L(x, y) do { (y)[3] = (uint8_t)(((x) >> 24) & 0xff); \
|
|
(y)[2] = (uint8_t)(((x) >> 16) & 0xff); \
|
|
(y)[1] = (uint8_t)(((x) >> 8) & 0xff); \
|
|
(y)[0] = (uint8_t)(((x) >> 0) & 0xff); } while (0)
|
|
|
|
#define LOAD32L(x, y) do { (x) = ((uint32_t)((y)[3] & 0xff) << 24) | \
|
|
((uint32_t)((y)[2] & 0xff) << 16) | \
|
|
((uint32_t)((y)[1] & 0xff) << 8) | \
|
|
((uint32_t)((y)[0] & 0xff) << 0); } while (0)
|
|
|
|
/* This function takes 64 bits of data in block and the 128 bits key in key */
|
|
static inline void csp_xtea_encrypt_block(uint8_t *block, uint8_t const *key) {
|
|
|
|
uint32_t i, v0, v1, delta = 0x9E3779B9, sum = 0, k[4];
|
|
|
|
LOAD32L(k[0], &key[0]);
|
|
LOAD32L(k[1], &key[4]);
|
|
LOAD32L(k[2], &key[8]);
|
|
LOAD32L(k[3], &key[12]);
|
|
|
|
LOAD32L(v0, &block[0]);
|
|
LOAD32L(v1, &block[4]);
|
|
|
|
for (i = 0; i < XTEA_ROUNDS; i++) {
|
|
v0 += (((v1 << 4) ^ (v1 >> 5)) + v1) ^ (sum + k[sum & 3]);
|
|
sum += delta;
|
|
v1 += (((v0 << 4) ^ (v0 >> 5)) + v0) ^ (sum + k[(sum >> 11) & 3]);
|
|
}
|
|
|
|
STORE32L(v0, &block[0]);
|
|
STORE32L(v1, &block[4]);
|
|
|
|
}
|
|
|
|
static inline void csp_xtea_xor_byte(uint8_t * dst, uint8_t * src, uint32_t len) {
|
|
|
|
unsigned int i;
|
|
for (i = 0; i < len; i++)
|
|
dst[i] ^= src[i];
|
|
|
|
}
|
|
|
|
int csp_xtea_set_key(char * key, uint32_t keylen) {
|
|
|
|
/* Use SHA1 as KDF */
|
|
uint8_t hash[SHA1_DIGESTSIZE];
|
|
csp_sha1_memory((uint8_t *)key, keylen, hash);
|
|
|
|
/* Copy key */
|
|
memcpy(csp_xtea_key, hash, XTEA_KEY_LENGTH);
|
|
|
|
return CSP_ERR_NONE;
|
|
|
|
}
|
|
|
|
int csp_xtea_encrypt(uint8_t * plain, const uint32_t len, uint32_t iv[2]) {
|
|
|
|
unsigned int i;
|
|
uint32_t stream[2];
|
|
|
|
uint32_t blocks = (len + XTEA_BLOCKSIZE - 1)/ XTEA_BLOCKSIZE;
|
|
uint32_t remain;
|
|
|
|
/* Initialize stream */
|
|
stream[0] = csp_htobe32(iv[0]);
|
|
stream[1] = csp_htobe32(iv[1]);
|
|
|
|
for (i = 0; i < blocks; i++) {
|
|
/* Create stream */
|
|
csp_xtea_encrypt_block((uint8_t *)stream, (uint8_t *)csp_xtea_key);
|
|
|
|
/* Calculate remaining bytes */
|
|
remain = len - i * XTEA_BLOCKSIZE;
|
|
|
|
/* XOR plain text with stream to generate cipher text */
|
|
csp_xtea_xor_byte(&plain[len - remain], (uint8_t *)stream, remain < XTEA_BLOCKSIZE ? remain : XTEA_BLOCKSIZE);
|
|
|
|
/* Increment counter */
|
|
stream[0] = csp_htobe32(iv[0]);
|
|
stream[1] = csp_htobe32(iv[1]++);
|
|
}
|
|
|
|
return CSP_ERR_NONE;
|
|
|
|
}
|
|
|
|
int csp_xtea_decrypt(uint8_t * cipher, const uint32_t len, uint32_t iv[2]) {
|
|
|
|
/* Since we use counter mode, we can reuse the encryption function */
|
|
return csp_xtea_encrypt(cipher, len, iv);
|
|
|
|
}
|
|
|
|
#endif // CSP_USE_XTEA
|