/*
 * This utility will read a string of RFM APDU commands from stdin
 * and wrap this OTA command block into an SMS-PP User Data element
 * as appropriate for sysmoUSIM-SJS1 version of SIM RFM, including
 * the necessary cryptographic checksum and ciphering.
 *
 * The two command line arguments are KIC2 and KID2 secret keys
 * for the target SIM, the payload to be wrapped is supplied on stdin.
 */

#include <sys/types.h>
#include <openssl/des.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>

#define	MAX_PAYLOAD_LEN	106

#define	MAX_SIGBUF	(MAX_PAYLOAD_LEN + 16 + 6)
#define	MAX_CIPHERTEXT	(MAX_PAYLOAD_LEN + 14)

u_char payload_data[MAX_PAYLOAD_LEN];
unsigned payload_len, padding_len;
int padding_mode;

DES_key_schedule keysched_kic[2], keysched_kid[2];

u_char part_head[8] = {0x15, 0x06, 0x01, 0x25, 0x25, 0xB0, 0x00, 0x10};

u_char part_cnt[6], signature[8];
u_char ciphertext[MAX_CIPHERTEXT];
unsigned ciphertext_len;

process_key_arg(arg, sched, name)
	char *arg, *name;
	DES_key_schedule *sched;
{
	DES_cblock keybin[2];
	int rc;
	unsigned n;

	rc = decode_hex_data_from_string(arg, keybin, 16, 16);
	if (rc < 0)
		exit(1);	/* error msg already printed */
	for (n = 0; n < 2; n++)
		DES_set_key_unchecked(keybin + n, sched + n);
}

read_payload()
{
	int rc;
	unsigned maxlen;

	maxlen = MAX_PAYLOAD_LEN;
	if (padding_mode)
		maxlen -= 8;
	rc = read_hex_from_stdin(payload_data, maxlen);
	if (rc < 0)
		exit(1);	/* error msg already printed */
	payload_len = rc;
}

compute_pad_length()
{
	unsigned ciph_len, rem;

	ciph_len = payload_len + 14;
	rem = ciph_len & 7;
	if (!rem && !padding_mode)
		padding_len = 0;
	else
		padding_len = 8 - rem;
}

compute_signature()
{
	u_char plainbuf[MAX_SIGBUF], cipher_out[MAX_SIGBUF];
	unsigned ciphlen;
	DES_cblock ivec;

	plainbuf[0] = 0;
	plainbuf[1] = payload_len + padding_len + 8 + 8 + 6;
	bcopy(part_head, plainbuf + 2, 8);
	bcopy(part_cnt, plainbuf + 10, 6);
	bcopy(payload_data, plainbuf + 16, payload_len + padding_len);
	bzero(plainbuf + 16 + payload_len + padding_len, 6);
	ciphlen = payload_len + padding_len + 16 + 6;
	bzero(&ivec, sizeof ivec);
	DES_ede2_cbc_encrypt(plainbuf, cipher_out, ciphlen, &keysched_kid[0],
			     &keysched_kid[1], &ivec, DES_ENCRYPT);
	bcopy(cipher_out + ciphlen - 8, signature, 8);
}

encrypt_message()
{
	u_char plainbuf[MAX_CIPHERTEXT];
	DES_cblock ivec;

	bcopy(part_cnt, plainbuf, 6);
	bcopy(signature, plainbuf + 6, 8);
	bcopy(payload_data, plainbuf + 14, payload_len + padding_len);
	ciphertext_len = payload_len + padding_len + 14;
	bzero(&ivec, sizeof ivec);
	DES_ede2_cbc_encrypt(plainbuf, ciphertext, ciphertext_len,
			     &keysched_kic[0], &keysched_kic[1], &ivec,
			     DES_ENCRYPT);
}

emit_hex_bytes(bytes, len)
	u_char *bytes;
	unsigned len;
{
	u_char *dp;
	unsigned n;

	dp = bytes;
	for (n = 0; n < len; n++)
		printf("%02X", *dp++);
}

main(argc, argv)
	char **argv;
{
	if (argc < 3 || argc > 4) {
		fprintf(stderr, "usage: %s KIC2-hex KID2-hex\n", argv[0]);
		exit(1);
	}
	process_key_arg(argv[1], keysched_kic, "KIC2");
	process_key_arg(argv[2], keysched_kid, "KID2");
	if (argc > 3)
		padding_mode = atoi(argv[3]);
	else
		padding_mode = 0;
	read_payload();
	compute_pad_length();
	part_cnt[5] = padding_len;
	compute_signature();
	encrypt_message();
	/* emit output! */
	printf("027000%04X", ciphertext_len + 8);
	emit_hex_bytes(part_head, 8);
	emit_hex_bytes(ciphertext, ciphertext_len);
	putchar('\n');
	exit(0);
}