//
//  bt_avdtp_acceptor.c
//
//  Bluetooth Protocol Stack - AVDTP ACP & SNK server
//  Copyright (C) 2017-2018 Toyohiko Togashi tog001@nifty.com
//
//
//  This program is free software; you can redistribute it and/or modify it under the terms of the
//  GNU 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 General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along with this program.
//  If not, see <http://www.gnu.org/licenses/>
//
//
//  Reference:
//      Bluetooth SIG (www.bluetooth.com)
//          Adopted Specifications
//          Protocols
//              AVDTP A/V Distribution Transport 1.3
//                Service Capabilities
//                  Media Transport     Support
//                  Reporting           Not support
//                  Recovery            Not support
//                  Content Protection  Not support
//                  Header Compression  Not support
//                  Multiplexing        Not support
//                  Media Codec         Support (SBC only)
//                  Delay Reporting     Not support
//
//  Update history
//  ---------- ----- -------------------------------------------
//  2017.03.10 v0.0  First cording
//  2018.11.18 v0.4c Bug fix (AVDTP disconnect state)
//

#include "bt_avdtp_acceptor.h"
#include "bt_spec_avdtp.h"
//#include "bt_spec_a2dp.h"
#include "bt_spec_l2cap.h"		// PSM

//#define DEBUG
#define MAX(a,b)    ((a) > (b) ? (a) : (b))
#define MIN(a,b)    ((a) < (b) ? (a) : (b))

//
//  Debug
//
#ifdef DEBUG
#include <stdio.h>                      // Debug
#define DEBUGP(...)     printf(__VA_ARGS__)
#else
#define DEBUGP(...)
#endif

//
//  Global area
//
struct bt_avdtp_ifa {	// Max 8bytes
	unsigned char	    channelType;
	unsigned char	    seid;
	unsigned short      seq;
	enum avdtp_state    state;
};
#define AVDTP_CHANNEL_TYPE_NA			0
#define AVDTP_CHANNEL_TYPE_SIGNALING	1
#define AVDTP_CHANNEL_TYPE_MEDIA		2
#define AVDTP_CHANNEL_MODE_REPORTING	0b00000001
#define AVDTP_CHANNEL_MODE_RECOVERY		0b00000010
#define AVDTP_CHANNEL_MODE_MULTIPLEXING	0b00000100
#define AVDTP_CHANNEL_MODE_HEADER_COMP	0b00001000


//
//  Internal subroutines
//
size_t bt_avdtpAcceptorSignaling(struct bt_l2capChannel *ch, unsigned char *inBuff, size_t inLen, unsigned char *outBuff, size_t outSize)
{
    size_t					len;
	struct bt_avdtp_ifa		*ifa;
	struct bt_avdtp_signal	*in;
	struct bt_avdtp_signal	*out;

    ifa = (struct bt_avdtp_ifa *)(ch->dynamic.userArea);
    in  = (struct bt_avdtp_signal *)inBuff;		// single packets only
    out = (struct bt_avdtp_signal *)outBuff;

    len = 0;
	if (inLen > 0) {
		unsigned char	transactionLabel;
		unsigned char	messageType;

		transactionLabel = in->header & AVDTP_HEADER_TransactionLabel_Mask;
		messageType      = in->header & AVDTP_HEADER_MessageType_Mask;
		DEBUGP("AVDTP: Signaling: label=%d msgType=%d id=%d\n", transactionLabel, messageType, in->signalIdentifier);

		// State less messages
		if (messageType == AVDTP_HEADER_MessageType_Command) {
			switch(in->signalIdentifier) {
			case AVDTP_DISCOVER:
			{
				int i;

				out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
				out->signalIdentifier = in->signalIdentifier;
				i   = bt_avdt_discover(out->streamEndPointDiscoveryResponse.param);
				len = AVDTP_signal_HeadSize + i;
				break;
			}
			case AVDTP_GET_CAPABILITIES:
			case AVDTP_GET_ALL_CAPABILITIES:
			{
				struct bt_avdtp_serviceCapabilitie *p;
				int i;
				int s;

				s = (in->getCapabilitiesCommand.param & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;

				out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
				out->signalIdentifier = in->signalIdentifier;
				len = AVDTP_signal_HeadSize;

				p = out->getCapabilitiesResponse.serviceCapabilities;
				p->serviceCategory = AVDTP_SERVICE_CATEGORY_MediaTransport;
				p->LOSC            = 0;
				len += AVDTP_serviceCapabilitie_HeadSize;

				p = (struct bt_avdtp_serviceCapabilitie *)&(p->mediaTransport.noParam);
				i = bt_avdt_getCapabilities(s, p, outSize-len);
				len += i;
				break;
			}
			case AVDTP_GET_CONFIGURATION:
			{
				struct bt_avdtp_serviceCapabilitie *p;
				int i;
				int s;

				out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
				out->signalIdentifier = in->signalIdentifier;
				len = AVDTP_signal_HeadSize;

				p = out->getConfigurationResponse.serviceCapabilities;
				p->serviceCategory = AVDTP_SERVICE_CATEGORY_MediaTransport;
				p->LOSC            = 0;
				len += AVDTP_serviceCapabilitie_HeadSize;

				p = (struct bt_avdtp_serviceCapabilitie *)&(p->mediaTransport.noParam);
				s = (in->getConfigurationCommand.param & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;
				i = bt_avdt_getConfiguration(s, p, outSize-len);
				if (i > 0) {
					len += i;
				} else {
					out->header = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
					out->getConfigurationReject.errorCode = -i;
					len = AVDTP_signal_HeadSize + sizeof(out->getConfigurationReject);
				}
				break;
			}
			default:
				break;
			}
		}

		if (len == 0) {
			switch(ifa->state){
			case AVDTP_STATE_IDLE:
				if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_SET_CONFIGURATION)) {
					int a;
					int i;
					int l;
					struct bt_avdtp_serviceCapabilitie *p;

					a = (in->setConfigurationCommand.param1 & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;		// ACP
					i = (in->setConfigurationCommand.param2 & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;		// INT
					p = (struct bt_avdtp_serviceCapabilitie *)&(in->setConfigurationCommand.serviceCapabilities);
					l = bt_avdt_setConfiguration(a, i, p, inLen - AVDTP_signal_HeadSize - sizeof(in->setConfigurationCommand));
					if (l >= 0) {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
						out->signalIdentifier = in->signalIdentifier;
						len = AVDTP_signal_HeadSize;
						ifa->seid  = a;
						ifa->state = AVDTP_STATE_CONFIGURED;
					} else {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
						out->signalIdentifier = in->signalIdentifier;
						out->setConfigurationReject.serviceCategory = 0;
						out->setConfigurationReject.errorCode       = -l;
						len = AVDTP_signal_HeadSize + sizeof(out->setConfigurationReject);
					}
				} else {
					out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_GeneralReject;
					out->signalIdentifier = in->signalIdentifier;
					len = AVDTP_signal_HeadSize + sizeof(out->setConfigurationReject);
				}
				break;
			case AVDTP_STATE_CONFIGURED:
				if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_OPEN)) {
					int a;
					int l;

					a = (in->openStreamCommand.param & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;		// ACP SEID
					l = bt_avdt_open(a);
					if (l >= 0) {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
						out->signalIdentifier = in->signalIdentifier;
						len = AVDTP_signal_HeadSize;
						ifa->seid  = a;
						ifa->state = AVDTP_STATE_OPEN;
					} else {
						out->header                     = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
						out->signalIdentifier           = in->signalIdentifier;
						out->openStreamReject.errorCode = -l;
						len = AVDTP_signal_HeadSize + sizeof(out->openStreamReject);
					}
				} else if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_ABORT)) {
					out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
					out->signalIdentifier = in->signalIdentifier;
					len = AVDTP_signal_HeadSize;
					ifa->state = AVDTP_STATE_ABORTING;
				} else {
					out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_GeneralReject;
					out->signalIdentifier = in->signalIdentifier;
					len = AVDTP_signal_HeadSize + sizeof(out->setConfigurationReject);
				}
				break;
			case AVDTP_STATE_OPEN:
				if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_START)) {
					int a;
					int l;
					int i;

					l = 0;
					a = 0;
					for(i = 0; i < (inLen - AVDTP_signal_HeadSize - sizeof(in->startStreamCommand)); i++) {
						a = (in->startStreamCommand.param[i] & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;		// ACP SEID
						l = bt_avdt_start(a);
						if (l < 0) {
							break;
						}
					}
					if (l >= 0) {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
						out->signalIdentifier = in->signalIdentifier;
						len = AVDTP_signal_HeadSize;
						ifa->state = AVDTP_STATE_STREAMING;
					} else {
						out->header                      = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
						out->signalIdentifier            = in->signalIdentifier;
						out->startStreamReject.param     = a << AVDTP_SEID_Shift;
						out->startStreamReject.errorCode = -l;
						len = AVDTP_signal_HeadSize + sizeof(out->startStreamReject);
					}
				} else if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_CLOSE)) {
					int a;
					int l;

					a = (in->closeStreamCommand.param & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;	// ACP SEID
					l = bt_avdt_close(a);
					if (l >= 0) {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
						out->signalIdentifier = in->signalIdentifier;
						len = AVDTP_signal_HeadSize + sizeof(out->closeStreamResponse);
						ifa->state = AVDTP_STATE_IDLE;
					} else {
						out->header                      = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
						out->signalIdentifier            = in->signalIdentifier;
						out->closeStreamReject.errorCode = -l;
						len = AVDTP_signal_HeadSize + sizeof(out->closeStreamReject);
					}
				} else if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_RECONFIGURE)) {
					int a;
					int l;
					struct bt_avdtp_serviceCapabilitie *p;

					a = (in->reconfigureCommand.param & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;		// ACP SEID
					p = (struct bt_avdtp_serviceCapabilitie *)&(in->reconfigureCommand.serviceCapabilities);
					l = bt_avdt_setReconfiguration(a, p, inLen - AVDTP_signal_HeadSize - sizeof(in->reconfigureCommand));
					if (l >= 0) {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
						out->signalIdentifier = in->signalIdentifier;
						len = AVDTP_signal_HeadSize;
					} else {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
						out->signalIdentifier = in->signalIdentifier;
						out->reconfigureReject.serviceCategory = 0;
						out->reconfigureReject.errorCode       = -l;
						len = AVDTP_signal_HeadSize + sizeof(out->setConfigurationReject);
					}
				} else if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_ABORT)) {
					out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
					out->signalIdentifier = in->signalIdentifier;
					len = AVDTP_signal_HeadSize;
					ifa->state = AVDTP_STATE_ABORTING;
				} else {
					out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_GeneralReject;
					out->signalIdentifier = in->signalIdentifier;
					len = AVDTP_signal_HeadSize + sizeof(out->setConfigurationReject);
				}
				break;
			case AVDTP_STATE_STREAMING:
				if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_CLOSE)) {
					ifa->state = AVDTP_STATE_CLOSING;
				} else if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_SUSPEND)) {
					int a;
					int l;
					int i;

					l = 0;
					a = 0;
					for(i = 0; i < (inLen - AVDTP_signal_HeadSize - sizeof(in->suspendCommand)); i++) {
						a = (in->suspendCommand.param[i] & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;	// ACP SEID
						l = bt_avdt_suspend(a);
						if (l < 0) {
							break;
						}
					}
					if (l >= 0) {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
						out->signalIdentifier = in->signalIdentifier;
						len = AVDTP_signal_HeadSize;
						ifa->state = AVDTP_STATE_OPEN;
					} else {
						out->header                  = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
						out->signalIdentifier        = in->signalIdentifier;
						out->suspendReject.param     = a << AVDTP_SEID_Shift;
						out->suspendReject.errorCode = -l;
						len = AVDTP_signal_HeadSize + sizeof(out->suspendReject);
					}
				} else if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_CLOSE)) {
					int a;
					int l;

					a = (in->closeStreamCommand.param & AVDTP_SEID_Mask) >> AVDTP_SEID_Shift;	// ACP SEID
					l = bt_avdt_close(a);
					if (l >= 0) {
						out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
						out->signalIdentifier = in->signalIdentifier;
						len = AVDTP_signal_HeadSize + sizeof(out->closeStreamResponse);
						ifa->state = AVDTP_STATE_IDLE;
					} else {
						out->header                      = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseReject;
						out->signalIdentifier            = in->signalIdentifier;
						out->closeStreamReject.errorCode = -l;
						len = AVDTP_signal_HeadSize + sizeof(out->closeStreamReject);
					}
				} else if ((messageType == AVDTP_HEADER_MessageType_Command) && (in->signalIdentifier == AVDTP_ABORT)) {
					out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_ResponseAccept;
					out->signalIdentifier = in->signalIdentifier;
					len = AVDTP_signal_HeadSize;
					ifa->state = AVDTP_STATE_ABORTING;
				} else {
					out->header           = transactionLabel | AVDTP_HEADER_PacketType_SinglePacket | AVDTP_HEADER_MessageType_GeneralReject;
					out->signalIdentifier = in->signalIdentifier;
					len = AVDTP_signal_HeadSize + sizeof(out->setConfigurationReject);
				}
				break;
			case AVDTP_STATE_CLOSING:
				break;
			case AVDTP_STATE_ABORTING:
				// Release of a resource
				bt_avdt_abort(ifa->seid);
				ifa->state = AVDTP_STATE_IDLE;
				break;
			default:
				break;
			}
		}
	}
	return(len);
}

size_t   bt_avdtpAcceptorStreamManager(struct bt_l2capChannel *ch, unsigned char *inBuff, size_t inLen, unsigned char *outBuff, size_t outSize)
{
	if (inLen > 0) {
		struct bt_avdtp_ifa	*ifa;
		struct bt_avdtp_media_packet_PDU_Format *in;
		int				cc;
		int				h;
		unsigned short	seq;

		ifa = (struct bt_avdtp_ifa *)(ch->dynamic.userArea);
		in  = (struct bt_avdtp_media_packet_PDU_Format *)inBuff;
		cc  = in->info1 & AVDTP_MEDIA_info1_CSRCcount_Mask;
		h   = sizeof(struct bt_avdtp_media_packet_PDU_Format) + cc * 4;
		seq = in->sequenceNumber[0] << 8 | in->sequenceNumber[1];

		DEBUGP("AVDTP: StreamManager: version=%02x padding=%d extension=%d cc=%d payload=%d seq=%d\n",
				(in->info1 & AVDTP_MEDIA_info1_Version_Mask),
				(in->info1 & AVDTP_MEDIA_info1_Padding_Mask),
				(in->info1 & AVDTP_MEDIA_info1_Extension_Mask),
				cc,
				(in->info2 & AVDTP_MEDIA_info2_PayloadType_Mask),
				seq);
		if (ifa->seq != seq) {
			bt_avdt_mediaRecieve(ifa->seid, &(in->CSRC[cc]), inLen - h);
			ifa->seq = seq;
		}
	}
	return(0);
}

//
//  Server
//
size_t bt_avdtpAcceptor(struct bt_l2capChannel *ch, unsigned char *inBuff, size_t inLen, unsigned char *outBuff, size_t outSize)
{
    size_t l;
	struct bt_avdtp_ifa	*ifa;

    ifa = (struct bt_avdtp_ifa *)(ch->dynamic.userArea);
    if (inLen > 0) {
    	// Selecting a "channel type" at the new channel
    	if (ifa->channelType == 0) {
    		struct bt_l2capChannel *a;
    		struct bt_l2capChannel *b;
    		int i;
    		int seid;

    		// Count of AVDTP channel in bluetooth-link/device
    		a = NULL;
    		i = 0;
    		seid = 0;
    		while((b = bt_l2capChannelGetNext(&a)) != NULL) {
    			if ((b->link         == ch->link        ) &&
					(b->service->psm == ch->service->psm)) {
    				struct bt_avdtp_ifa	*a;

    				i++;
    			    a = (struct bt_avdtp_ifa *)(b->dynamic.userArea);
    				if ((a->channelType == AVDTP_CHANNEL_TYPE_SIGNALING) &&
    					(a->seid        != 0                           )) {
    					seid = a->seid;
    				}
    			}
    		}
			if (i <= 1) {
				ifa->channelType = AVDTP_CHANNEL_TYPE_SIGNALING;
			} else {
				ifa->channelType = AVDTP_CHANNEL_TYPE_MEDIA;
				ifa->seid        = seid;
			}
    	}
    }
	switch(ifa->channelType) {
	case AVDTP_CHANNEL_TYPE_SIGNALING:
		l = bt_avdtpAcceptorSignaling(ch, inBuff, inLen, outBuff, outSize);
		break;
	case AVDTP_CHANNEL_TYPE_MEDIA:
		l = bt_avdtpAcceptorStreamManager(ch, inBuff, inLen, outBuff, outSize);
		break;
	default:
		l = 0;
		break;
	}

	return(l);
}
