--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/smswrap/ota-smswrap-sjs1.c	Sun Feb 21 22:21:29 2021 +0000
@@ -0,0 +1,153 @@
+/*
+ * 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++) {
+		rc = DES_set_key_checked(keybin + n, sched + n);
+		if (rc < 0) {
+			fprintf(stderr,
+	"bad %s argument: DES_set_key_checked() returned %d on part %u\n",
+				name, rc, n);
+			exit(1);
+		}
+	}
+}
+
+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);
+}