/* GPRS LLC protocol implementation as per 3GPP TS 44.064 */

/* (C) 2009-2010 by Harald Welte <laforge@gnumonks.org>
 * (C) 2022 by Sysmocom s.f.m.c. GmbH
 *
 * All Rights Reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdint.h>
#include <errno.h>

#include <osmocom/core/msgb.h>
#include <osmocom/core/utils.h>
#include <osmocom/core/logging.h>

#include <osmocom/gprs/llc/llc.h>
#include <osmocom/gprs/llc/llc_private.h>

const struct value_string osmo_gprs_llc_sapi_names[] = {
	{ OSMO_GPRS_LLC_SAPI_GMM,	"GMM" },
	{ OSMO_GPRS_LLC_SAPI_TOM2,	"TOM2" },
	{ OSMO_GPRS_LLC_SAPI_SNDCP3,	"SNDCP3" },
	{ OSMO_GPRS_LLC_SAPI_SNDCP5,	"SNDCP5" },
	{ OSMO_GPRS_LLC_SAPI_SMS,	"SMS" },
	{ OSMO_GPRS_LLC_SAPI_TOM8,	"TOM8" },
	{ OSMO_GPRS_LLC_SAPI_SNDCP9,	"SNDCP9" },
	{ OSMO_GPRS_LLC_SAPI_SNDCP11,	"SNDCP11" },
	{ 0, NULL }
};

const struct value_string gprs_llc_frame_fmt_names[] = {
	{ OSMO_GPRS_LLC_FMT_I,		"I" },
	{ OSMO_GPRS_LLC_FMT_S,		"U" },
	{ OSMO_GPRS_LLC_FMT_UI,		"UI" },
	{ OSMO_GPRS_LLC_FMT_U,		"U" },
	{ 0, NULL }
};

const struct value_string gprs_llc_frame_func_names[] = {
	/* 6.4.1 Unnumbered (U) frames */
	{ OSMO_GPRS_LLC_FUNC_SABM,	"SABM" },
	{ OSMO_GPRS_LLC_FUNC_DISC,	"DISC" },
	{ OSMO_GPRS_LLC_FUNC_UA,	"UA" },
	{ OSMO_GPRS_LLC_FUNC_DM,	"DM" },
	{ OSMO_GPRS_LLC_FUNC_FRMR,	"FRMR" },
	{ OSMO_GPRS_LLC_FUNC_XID,	"XID" },
	{ OSMO_GPRS_LLC_FUNC_NULL,	"NULL" },
	/* 6.4.2 Unconfirmed Information (UI) frame */
	{ OSMO_GPRS_LLC_FUNC_UI,	"UI" },
	{ OSMO_GPRS_LLC_FUNC_UI_DUMMY,	"UI Dummy" },
	/* 6.4.3 Combined Information (I) and Supervisory (S) frames */
	{ OSMO_GPRS_LLC_FUNC_RR,	"RR" },
	{ OSMO_GPRS_LLC_FUNC_ACK,	"ACK" },
	{ OSMO_GPRS_LLC_FUNC_SACK,	"SACK" },
	{ OSMO_GPRS_LLC_FUNC_RNR,	"RNR" },
	{ 0, NULL }
};

uint32_t crc24_calc(uint32_t fcs, const uint8_t *data, size_t len);

uint32_t gprs_llc_fcs(const uint8_t *data, size_t len)
{
	uint32_t fcs_calc;

	fcs_calc = crc24_calc(0xffffff, data, len);
	fcs_calc = ~fcs_calc;
	fcs_calc &= 0xffffff;

	return fcs_calc;
}

void gprs_llc_pdu_hdr_dump_buf(char *buf, size_t buf_size,
			       const struct gprs_llc_pdu_decoded *pdu)
{
	struct osmo_strbuf sb = { .buf = buf, .len = buf_size };

	OSMO_STRBUF_PRINTF(sb, "SAPI=%u (%s), %s func=%s C/R=%c",
			   pdu->sapi, osmo_gprs_llc_sapi_name(pdu->sapi),
			   gprs_llc_frame_fmt_name(pdu->fmt),
			   gprs_llc_frame_func_name(pdu->func),
			   pdu->flags & OSMO_GPRS_LLC_PDU_F_CMD_RSP ? '1' : '0');

	switch (pdu->fmt) {
	case OSMO_GPRS_LLC_FMT_I:
		OSMO_STRBUF_PRINTF(sb, " A=%c N(R)=%u N(S)=%u",
				   pdu->flags & OSMO_GPRS_LLC_PDU_F_ACK_REQ ? '1' : '0',
				   pdu->seq_rx, pdu->seq_tx);
		break;
	case OSMO_GPRS_LLC_FMT_S:
		OSMO_STRBUF_PRINTF(sb, " A=%c N(R)=%u",
				   pdu->flags & OSMO_GPRS_LLC_PDU_F_ACK_REQ ? '1' : '0',
				   pdu->seq_rx);
		break;
	case OSMO_GPRS_LLC_FMT_UI:
		OSMO_STRBUF_PRINTF(sb, " PM=%c E=%c IP=%c N(U)=%u",
				   pdu->flags & OSMO_GPRS_LLC_PDU_F_PROT_MODE ? '1' : '0',
				   pdu->flags & OSMO_GPRS_LLC_PDU_F_ENC_MODE ? '1' : '0',
				   pdu->flags & OSMO_GPRS_LLC_PDU_F_MAC_PRES ? '1' : '0',
				   pdu->seq_tx);
		break;
	case OSMO_GPRS_LLC_FMT_U:
		OSMO_STRBUF_PRINTF(sb, " P/F=%c",
				   pdu->flags & OSMO_GPRS_LLC_PDU_F_FOLL_FIN ? '1' : '0');
		break;
	}

	if (pdu->flags & OSMO_GPRS_LLC_PDU_F_MAC_PRES)
		OSMO_STRBUF_PRINTF(sb, " MAC=%08x", pdu->mac);
	OSMO_STRBUF_PRINTF(sb, " FCS=%06x", pdu->fcs);
}

const char *gprs_llc_pdu_hdr_dump(const struct gprs_llc_pdu_decoded *pdu)
{
	static __thread char buf[256];
	gprs_llc_pdu_hdr_dump_buf(&buf[0], sizeof(buf), pdu);
	return buf;
}

/* 6.4.1 Unnumbered (U) frames */
#define GPRS_LLC_U_NULL_CMD		0x00
#define GPRS_LLC_U_DM_RESP		0x01
#define GPRS_LLC_U_DISC_CMD		0x04
#define GPRS_LLC_U_UA_RESP		0x06
#define GPRS_LLC_U_SABM_CMD		0x07
#define GPRS_LLC_U_FRMR_RESP		0x08
#define GPRS_LLC_U_XID			0x0b

int gprs_llc_pdu_encode(struct msgb *msg, const struct gprs_llc_pdu_decoded *pdu)
{
	uint8_t *addr = msgb_put(msg, 1);
	uint8_t *ctrl = NULL;
	size_t crc_len;
	uint32_t fcs;

	/* 6.2.3 Service Access Point Identifier (SAPI) */
	addr[0] = pdu->sapi & 0x0f;

	/* 6.2.2 Commmand/Response bit (C/R) */
	if (pdu->flags & OSMO_GPRS_LLC_PDU_F_CMD_RSP)
		addr[0] |= (1 << 6);

	switch (pdu->fmt) {
	case OSMO_GPRS_LLC_FMT_I:
		ctrl = msgb_put(msg, 3);

		ctrl[0] = 0x00; /* 0xxxxxxx */
		ctrl[1] = ctrl[2] = 0x00;

		if (pdu->flags & OSMO_GPRS_LLC_PDU_F_ACK_REQ)
			ctrl[0] |= (1 << 6);

		ctrl[0] |= (pdu->seq_tx >> 4) & 0x1f;
		ctrl[1] |= (pdu->seq_tx & 0x0f) << 4;

		ctrl[1] |= (pdu->seq_rx >> 6) & 0x07;
		ctrl[2] |= (pdu->seq_rx & 0x3f) << 2;

		ctrl[2] |= (pdu->func - OSMO_GPRS_LLC_FUNC_RR) & 0x03;

		if (pdu->func == OSMO_GPRS_LLC_FUNC_SACK) {
			if (pdu->sack.len == 0)
				return -EINVAL;
			msgb_put_u8(msg, pdu->sack.len - 1);
			memcpy(msgb_put(msg, pdu->sack.len),
			       &pdu->sack.r[0], pdu->sack.len);
		}
		break;
	case OSMO_GPRS_LLC_FMT_S:
		ctrl = msgb_put(msg, 2);

		ctrl[0] = 0x80; /* 10xxxxxx */
		ctrl[1] = 0x00;

		if (pdu->flags & OSMO_GPRS_LLC_PDU_F_ACK_REQ)
			ctrl[1] |= 0x20;

		ctrl[0] |= (pdu->seq_rx >> 6) & 0x07;
		ctrl[1] |= (pdu->seq_rx & 0x3f) << 2;

		ctrl[1] |= (pdu->func - OSMO_GPRS_LLC_FUNC_RR) & 0x03;

		if (pdu->func == OSMO_GPRS_LLC_FUNC_SACK) {
			if (pdu->sack.len == 0)
				return -EINVAL;
			memcpy(msgb_put(msg, pdu->sack.len),
			       &pdu->sack.r[0], pdu->sack.len);
		}
		break;
	case OSMO_GPRS_LLC_FMT_UI:
		ctrl = msgb_put(msg, 2);

		ctrl[0] = 0xc0; /* 110xxxxx */
		ctrl[1] = 0x00;

		if (pdu->flags & OSMO_GPRS_LLC_PDU_F_MAC_PRES)
			ctrl[0] |= (1 << 4);

		ctrl[0] |= (pdu->seq_tx >> 6) & 0x07;
		ctrl[1] |= (pdu->seq_tx & 0x3f) << 2;

		if (pdu->flags & OSMO_GPRS_LLC_PDU_F_ENC_MODE)
			ctrl[1] |= (1 << 1);
		if (pdu->flags & OSMO_GPRS_LLC_PDU_F_PROT_MODE)
			ctrl[1] |= (1 << 0);
		break;
	case OSMO_GPRS_LLC_FMT_U:
		ctrl = msgb_put(msg, 1);

		ctrl[0] = 0xe0; /* 111xxxxx */

		if (pdu->flags & OSMO_GPRS_LLC_PDU_F_FOLL_FIN)
			ctrl[0] |= (1 << 4);

		switch (pdu->func) {
		case OSMO_GPRS_LLC_FUNC_NULL:
			ctrl[0] |= GPRS_LLC_U_NULL_CMD;
			break;
		case OSMO_GPRS_LLC_FUNC_DM:
			ctrl[0] |= GPRS_LLC_U_DM_RESP;
			break;
		case OSMO_GPRS_LLC_FUNC_DISC:
			ctrl[0] |= GPRS_LLC_U_DISC_CMD;
			break;
		case OSMO_GPRS_LLC_FUNC_UA:
			ctrl[0] |= GPRS_LLC_U_UA_RESP;
			break;
		case OSMO_GPRS_LLC_FUNC_SABM:
			ctrl[0] |= GPRS_LLC_U_SABM_CMD;
			break;
		case OSMO_GPRS_LLC_FUNC_FRMR:
			ctrl[0] |= GPRS_LLC_U_FRMR_RESP;
			break;
		case OSMO_GPRS_LLC_FUNC_XID:
			ctrl[0] |= GPRS_LLC_U_XID;
			break;
		default:
			LOGLLC(LOGL_ERROR, "Unknown UI func=0x%02x\n", pdu->func);
			return -EINVAL;
		}
		break;
	}

	if (pdu->data_len > 0) {
		uint8_t *data = msgb_put(msg, pdu->data_len);
		memcpy(data, pdu->data, pdu->data_len);
	}

	/* 5.5a Message Authentication Code (MAC) field */
	if (pdu->flags & OSMO_GPRS_LLC_PDU_F_MAC_PRES) {
		/* TODO: calculate MAC (see 3GPP TS 43.020) */
		LOGLLC(LOGL_ERROR, "Message Authentication Code (MAC) is not implemented\n");
		return -ENOTSUP;
	}

	/* 5.5 Frame Check Sequence (FCS) field */
	crc_len = msg->tail - addr;
	if (~pdu->flags & OSMO_GPRS_LLC_PDU_F_PROT_MODE)
		crc_len = OSMO_MIN(crc_len, UI_HDR_LEN + N202);
	fcs = gprs_llc_fcs(addr, crc_len);

	msgb_put_u8(msg, fcs & 0xff);
	msgb_put_u8(msg, (fcs >> 8) & 0xff);
	msgb_put_u8(msg, (fcs >> 16) & 0xff);

	return 0;
}

int gprs_llc_pdu_decode(struct gprs_llc_pdu_decoded *pdu,
			uint8_t *data, size_t data_len)
{
	uint8_t *addr = &data[0];
	uint8_t *ctrl = &data[1];

#define check_len(len, text) \
	do { \
		if (data_len < (len)) { \
			LOGLLC(LOGL_ERROR, "Failed to parse LLC PDU: %s\n", text); \
			return -EINVAL; \
		} \
	} while (0)

	/* 5.5 Frame Check Sequence (FCS) field */
	check_len(CRC24_LENGTH, "missing Frame Check Sequence (FCS) field");
	pdu->fcs  = data[data_len - 3];
	pdu->fcs |= data[data_len - 2] << 8;
	pdu->fcs |= data[data_len - 1] << 16;
	data_len -= CRC24_LENGTH;

	/* 6.2.0 Address field format */
	check_len(1, "missing Address field");
	data_len -= 1;

	/* Initial assumption: FCS covers hdr + all inf fields */
	pdu->flags |= OSMO_GPRS_LLC_PDU_F_PROT_MODE;

	/* 6.2.1 Protocol Discriminator bit (PD): shall be 0 */
	if (*addr & 0x80) {
		LOGLLC(LOGL_ERROR, "Protocol Discriminator shall be 0\n");
		return -EINVAL;
	}

	/* 6.2.2 Commmand/Response bit (C/R) */
	if (*addr & 0x40)
		pdu->flags |= OSMO_GPRS_LLC_PDU_F_CMD_RSP;

	/* 6.2.3 Service Access Point Identifier (SAPI) */
	pdu->sapi = *addr & 0x0f;

	/* Check for reserved SAPI */
	switch (*addr & 0x0f) {
	case 0x00:
	case 0x04:
	case 0x06:
	case 0x0a:
	case 0x0c:
	case 0x0d:
	case 0x0f:
		LOGLLC(LOGL_ERROR, "Unknown SAPI=%u\n", pdu->sapi);
		return -EINVAL;
	}

	/* U format has the shortest control field length=1 */
	check_len(1, "missing Control field");

	/* 6.3.0 Control field formats */
	if ((ctrl[0] & 0x80) == 0) {
		/* 6.3.1 Information transfer format - I */
		pdu->fmt = OSMO_GPRS_LLC_FMT_I;

		check_len(3, "I format Control field is too short");
		data_len -= 3;

		pdu->data = ctrl + 3;
		/* pdu->data_len is set below */

		if (ctrl[0] & 0x40)
			pdu->flags |= OSMO_GPRS_LLC_PDU_F_ACK_REQ;

		pdu->seq_tx  = (ctrl[0] & 0x1f) << 4;
		pdu->seq_tx |= (ctrl[1] >> 4);

		pdu->seq_rx  = (ctrl[1] & 0x7) << 6;
		pdu->seq_rx |= (ctrl[2] >> 2);

		switch (ctrl[2] & 0x03) {
		case 0:
			pdu->func = OSMO_GPRS_LLC_FUNC_RR;
			break;
		case 1:
			pdu->func = OSMO_GPRS_LLC_FUNC_ACK;
			break;
		case 2:
			pdu->func = OSMO_GPRS_LLC_FUNC_RNR;
			break;
		case 3:
			pdu->func = OSMO_GPRS_LLC_FUNC_SACK;
			check_len(1, "I func=SACK is too short");
			pdu->sack.len = (ctrl[3] & 0x1f) + 1; /* 1 .. 32 */
			/* The R(n) bitmask takes len=(K + 1) octets */
			check_len(pdu->sack.len, "I func=SACK is too short");
			memcpy(&pdu->sack.r[0], ctrl + 4, pdu->sack.len);
			pdu->data += 1 + pdu->sack.len;
			data_len -= 1 + pdu->sack.len;
			break;
		}
		pdu->data_len = data_len;
	} else if ((ctrl[0] & 0xc0) == 0x80) {
		/* 6.3.2 Supervisory format - S */
		pdu->fmt = OSMO_GPRS_LLC_FMT_S;

		check_len(2, "S format Control field is too short");
		data_len -= 2;

		pdu->data = NULL;
		pdu->data_len = 0;

		if (ctrl[0] & 0x20)
			pdu->flags |= OSMO_GPRS_LLC_PDU_F_ACK_REQ;

		pdu->seq_rx  = (ctrl[0] & 0x7) << 6;
		pdu->seq_rx |= (ctrl[1] >> 2);

		switch (ctrl[1] & 0x03) {
		case 0:
			pdu->func = OSMO_GPRS_LLC_FUNC_RR;
			break;
		case 1:
			pdu->func = OSMO_GPRS_LLC_FUNC_ACK;
			break;
		case 2:
			pdu->func = OSMO_GPRS_LLC_FUNC_RNR;
			break;
		case 3:
			pdu->func = OSMO_GPRS_LLC_FUNC_SACK;
			/* The R(n) bitmask takes all remaining octets */
			check_len(1, "S func=SACK is too short");
			pdu->sack.len = data_len; /* 1 .. 32 */
			memcpy(&pdu->sack.r[0], ctrl + 2, pdu->sack.len);
			break;
		}
	} else if ((ctrl[0] & 0xe0) == 0xc0) {
		/* 6.3.3 Unconfirmed Information format - UI */
		pdu->fmt = OSMO_GPRS_LLC_FMT_UI;
		pdu->func = OSMO_GPRS_LLC_FUNC_UI;

		check_len(2, "UI format Control field is too short");
		data_len -= 2;

		pdu->data = ctrl + 2;
		pdu->data_len = data_len;

		pdu->seq_tx  = (ctrl[0] & 0x7) << 6;
		pdu->seq_tx |= (ctrl[1] >> 2);

		if (ctrl[0] & 0x10) {
			check_len(sizeof(pdu->mac), "missing MAC field");
			pdu->data_len -= sizeof(pdu->mac);
			data_len -= sizeof(pdu->mac);

			pdu->mac = osmo_load32le(&pdu->data[data_len]);
			pdu->flags |= OSMO_GPRS_LLC_PDU_F_MAC_PRES;
		}

		if (ctrl[1] & 0x02)
			pdu->flags |= OSMO_GPRS_LLC_PDU_F_ENC_MODE;

		if (~ctrl[1] & 0x01) /* FCS covers hdr + N202 octets */
			pdu->flags &= ~OSMO_GPRS_LLC_PDU_F_PROT_MODE;
	} else {
		/* 6.3.4 Unnumbered format - U */
		pdu->fmt = OSMO_GPRS_LLC_FMT_U;

		check_len(1, "U format Control field is too short");
		data_len -= 1;

		pdu->data = NULL;
		pdu->data_len = 0;

		if (ctrl[0] & 0x10)
			pdu->flags |= OSMO_GPRS_LLC_PDU_F_FOLL_FIN;

		switch (ctrl[0] & 0x0f) {
		case GPRS_LLC_U_NULL_CMD:
			pdu->func = OSMO_GPRS_LLC_FUNC_NULL;
			break;
		case GPRS_LLC_U_DM_RESP:
			pdu->func = OSMO_GPRS_LLC_FUNC_DM;
			break;
		case GPRS_LLC_U_DISC_CMD:
			pdu->func = OSMO_GPRS_LLC_FUNC_DISC;
			break;
		case GPRS_LLC_U_UA_RESP:
			pdu->func = OSMO_GPRS_LLC_FUNC_UA;
			break;
		case GPRS_LLC_U_SABM_CMD:
			pdu->func = OSMO_GPRS_LLC_FUNC_SABM;
			break;
		case GPRS_LLC_U_FRMR_RESP:
			pdu->func = OSMO_GPRS_LLC_FUNC_FRMR;
			break;
		case GPRS_LLC_U_XID:
			pdu->func = OSMO_GPRS_LLC_FUNC_XID;
			pdu->data = ctrl + 1;
			pdu->data_len = data_len;
			break;
		default:
			LOGLLC(LOGL_ERROR, "Unknown U func=0x%02x\n", ctrl[0] & 0x0f);
			return -ENOTSUP;
		}
	}

#undef check_len

	return 0;
}