static void RFCOMM_ProcessTestCommand(const RFCOMM_Command_t* const CommandHeader, const uint8_t CommandDataLen, const uint8_t* CommandData, Bluetooth_Channel_t* const ACLChannel) { const uint8_t* Params = (const uint8_t*)CommandData; BT_RFCOMM_DEBUG(1, "<< TEST Command"); struct { RFCOMM_Command_t CommandHeader; uint8_t Length; uint8_t TestData[CommandDataLen]; } TestResponse; /* Fill out the Test response data */ TestResponse.CommandHeader = (RFCOMM_Command_t){.Command = RFCOMM_Control_Test, .EA = true, .CR = false}; TestResponse.Length = (CommandDataLen << 1) | 0x01; memcpy(TestResponse.TestData, Params, CommandDataLen); BT_RFCOMM_DEBUG(1, ">> TEST Response"); /* Send the PDN response to acknowledge the command */ RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, false, RFCOMM_Frame_UIH, sizeof(TestResponse), &TestResponse, ACLChannel); } static void RFCOMM_ProcessFCECommand(const RFCOMM_Command_t* const CommandHeader, const uint8_t* CommandData, Bluetooth_Channel_t* const ACLChannel) { BT_RFCOMM_DEBUG(1, "<< FCE Command"); }
static void RFCOMM_ProcessMSCCommand(const RFCOMM_Command_t* const CommandHeader, const uint8_t CommandDataLen, const uint8_t* CommandData, Bluetooth_Channel_t* const ACLChannel) { const RFCOMM_MSC_Parameters_t* Params = (const RFCOMM_MSC_Parameters_t*)CommandData; BT_RFCOMM_DEBUG(1, "<< MSC %s", (CommandHeader->CR) ? "Command" : "Response"); BT_RFCOMM_DEBUG(2, "-- DLCI: 0x%02X", Params->Channel.DLCI); /* Ignore status flags sent to the control channel */ if (Params->Channel.DLCI == RFCOMM_CONTROL_DLCI) return; /* Retrieve existing channel configuration data, if already opened */ RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(Params->Channel.DLCI); /* If the channel does not exist, abort */ if (RFCOMMChannel == NULL) return; /* Check if the MSC packet is a command or a response */ if (CommandHeader->CR) { /* Save the new channel signals to the channel state structure */ RFCOMMChannel->Remote.Signals = Params->Signals; RFCOMMChannel->ConfigFlags |= RFCOMM_CONFIG_REMOTESIGNALS; /* If the command contains the optional break signals field, store the value */ if (CommandDataLen == sizeof(RFCOMM_MSC_Parameters_t)) RFCOMMChannel->Remote.BreakSignal = Params->BreakSignal; /* Notify the user application that the signals have been received */ RFCOMM_ChannelSignalsReceived(RFCOMMChannel); struct { RFCOMM_Command_t CommandHeader; uint8_t Length; RFCOMM_MSC_Parameters_t Params; } MSResponse; /* Fill out the MS response data */ MSResponse.CommandHeader = (RFCOMM_Command_t){.Command = RFCOMM_Control_ModemStatus, .EA = true, .CR = false}; MSResponse.Length = (CommandDataLen << 1) | 0x01; memcpy(&MSResponse.Params, Params, sizeof(RFCOMM_MSC_Parameters_t)); BT_RFCOMM_DEBUG(1, ">> MSC Response"); /* Send the MSC response to acknowledge the command */ RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, false, RFCOMM_Frame_UIH, (sizeof(MSResponse) - sizeof(MSResponse.Params) + CommandDataLen), &MSResponse, ACLChannel); } else {
static void RFCOMM_ProcessDISC(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel) { BT_RFCOMM_DEBUG(1, "<< DISC Received"); BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI); RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI); /* If the requested channel is currently open, destroy it */ if (RFCOMMChannel != NULL) RFCOMMChannel->State = RFCOMM_Channel_Closed; BT_RFCOMM_DEBUG(1, ">> UA Sent"); RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel); }
void RFCOMM_ProcessControlCommand(const uint8_t* Command, Bluetooth_Channel_t* const ACLChannel) { const RFCOMM_Command_t* CommandHeader = (const RFCOMM_Command_t*)Command; const uint8_t* CommandData = (const uint8_t*)Command + sizeof(RFCOMM_Command_t); uint8_t CommandDataLen = RFCOMM_GetVariableFieldValue(&CommandData); switch (CommandHeader->Command) { case RFCOMM_Control_Test: RFCOMM_ProcessTestCommand(CommandHeader, CommandDataLen, CommandData, ACLChannel); break; case RFCOMM_Control_FlowControlEnable: RFCOMM_ProcessFCECommand(CommandHeader, CommandData, ACLChannel); break; case RFCOMM_Control_FlowControlDisable: RFCOMM_ProcessFCDCommand(CommandHeader, CommandData, ACLChannel); break; case RFCOMM_Control_ModemStatus: RFCOMM_ProcessMSCCommand(CommandHeader, CommandDataLen, CommandData, ACLChannel); break; case RFCOMM_Control_RemotePortNegotiation: RFCOMM_ProcessRPNCommand(CommandHeader, CommandData, ACLChannel); break; case RFCOMM_Control_RemoteLineStatus: RFCOMM_ProcessRLSCommand(CommandHeader, CommandData, ACLChannel); break; case RFCOMM_Control_DLCParameterNegotiation: RFCOMM_ProcessDPNCommand(CommandHeader, CommandData, ACLChannel); break; default: BT_RFCOMM_DEBUG(1, "<< Unknown Command"); break; } }
/** Processes an incoming RFCOMM packet on an ACL channel which has been previously opened between the local and * a remote device to handle RFCOMM traffic. * * \param[in] Data Incoming packet data containing the RFCOMM packet * \param[in] ACLChannel ACL channel the request was issued to by the remote device */ void RFCOMM_ProcessPacket(void* Data, Bluetooth_Channel_t* const ACLChannel) { const RFCOMM_Header_t* FrameHeader = (const RFCOMM_Header_t*)Data; const uint8_t* FrameData = (const uint8_t*)Data + sizeof(RFCOMM_Header_t); uint16_t FrameDataLen = RFCOMM_GetVariableFieldValue(&FrameData); /* Decode the RFCOMM frame type from the header */ switch (FrameHeader->Control & ~FRAME_POLL_FINAL) { case RFCOMM_Frame_DM: RFCOMM_ProcessDM(&FrameHeader->Address, ACLChannel); break; case RFCOMM_Frame_DISC: RFCOMM_ProcessDISC(&FrameHeader->Address, ACLChannel); break; case RFCOMM_Frame_SABM: RFCOMM_ProcessSABM(&FrameHeader->Address, ACLChannel); break; case RFCOMM_Frame_UA: RFCOMM_ProcessUA(&FrameHeader->Address, ACLChannel); break; case RFCOMM_Frame_UIH: RFCOMM_ProcessUIH(&FrameHeader->Address, FrameDataLen, FrameData, ACLChannel); break; default: BT_RFCOMM_DEBUG(1, "<< Unknown Frame Received"); break; } }
static void RFCOMM_ProcessUIH(const RFCOMM_Address_t* const FrameAddress, const uint16_t FrameLength, const uint8_t* FrameData, Bluetooth_Channel_t* const ACLChannel) { if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI) { RFCOMM_ProcessControlCommand(FrameData, ACLChannel); return; } BT_RFCOMM_DEBUG(1, "<< UIH Received"); BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI); BT_RFCOMM_DEBUG(2, "-- Length 0x%02X", FrameLength); RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI); if (RFCOMMChannel != NULL) RFCOMM_DataReceived(RFCOMMChannel, FrameLength, FrameData); }
static void RFCOMM_ProcessSABM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel) { BT_RFCOMM_DEBUG(1, "<< SABM Received"); BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI); if (FrameAddress->DLCI == RFCOMM_CONTROL_DLCI) { BT_RFCOMM_DEBUG(1, ">> UA Sent"); /* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */ RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel); return; } /* Find the existing channel's entry in the channel table */ RFCOMM_Channel_t* RFCOMMChannel = RFCOMM_GetChannelData(FrameAddress->DLCI); /* Existing entry not found, create a new entry for the channel */ if (RFCOMMChannel == NULL) RFCOMMChannel = RFCOMM_GetFreeChannelEntry(FrameAddress->DLCI); /* If space was found in the channel table for the new channel, ACK the request */ if (RFCOMMChannel != NULL) { BT_RFCOMM_DEBUG(1, ">> UA Sent"); /* Free channel found, or request was to the control channel - accept SABM by sending a UA frame */ RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_UA | FRAME_POLL_FINAL), 0, NULL, ACLChannel); } else { BT_RFCOMM_DEBUG(1, ">> DM Sent"); /* No free channel in the multiplexer - decline the SABM by sending a DM frame */ RFCOMM_SendFrame(FrameAddress->DLCI, true, (RFCOMM_Frame_DM | FRAME_POLL_FINAL), 0, NULL, ACLChannel); } }
static void RFCOMM_ProcessFCDCommand(const RFCOMM_Command_t* const CommandHeader, const uint8_t* CommandData, Bluetooth_Channel_t* const ACLChannel) { BT_RFCOMM_DEBUG(1, "<< FCD Command"); }
static void RFCOMM_ProcessUA(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel) { BT_RFCOMM_DEBUG(1, "<< UA Received"); BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI); }
/** Sends an RFCOMM notification to the remote device that the local terminal control signals (located in the * "Local" structure of the RFCOMM channel) have changed, pushing the new signals to the remote device. * * \param[in] RFCOMMChannel RFCOMM logical channel whose local terminal signals have changed * \param[in] ACLChannel ACL channel which has been opened to carry RFCOMM traffic between devices */ void RFCOMM_SendChannelSignals(const RFCOMM_Channel_t* const RFCOMMChannel, Bluetooth_Channel_t* const ACLChannel) { BT_RFCOMM_DEBUG(1, ">> MSC Command"); BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", RFCOMMChannel->DLCI); struct { RFCOMM_Command_t CommandHeader; uint8_t Length; RFCOMM_MSC_Parameters_t Params; } MSCommand; MSCommand.CommandHeader = (RFCOMM_Command_t){.Command = RFCOMM_Control_ModemStatus, .EA = true, .CR = true}; MSCommand.Length = (sizeof(MSCommand.Params) << 1) | 0x01; MSCommand.Params.Channel = (RFCOMM_Address_t){.DLCI = RFCOMMChannel->DLCI, .EA = true, .CR = true}; MSCommand.Params.Signals = RFCOMMChannel->Local.Signals; MSCommand.Params.BreakSignal = RFCOMMChannel->Local.BreakSignal; /* Send the MSC command to the remote device */ RFCOMM_SendFrame(RFCOMM_CONTROL_DLCI, true, RFCOMM_Frame_UIH, sizeof(MSCommand), &MSCommand, ACLChannel); } /** Sends new data through an open logical RFCOMM channel. This should be used to transmit data through a * RFCOMM channel once it has been opened. * * \param[in] DataLen Length of the RFCOMM data to send, in bytes * \param[in] Data Pointer to a buffer where the data to send is located * \param[in] RFCOMMChannel RFCOMM logical channel which is to be transmitted to * \param[in] ACLChannel ACL channel which has been opened to carry RFCOMM traffic between devices */ void RFCOMM_SendData(const uint16_t DataLen, const uint8_t* Data, const RFCOMM_Channel_t* const RFCOMMChannel, Bluetooth_Channel_t* const ACLChannel) { if (RFCOMMChannel->State != RFCOMM_Channel_Open) return; BT_RFCOMM_DEBUG(1, ">> UIH Frame"); BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", RFCOMMChannel->DLCI); /* Send the MSC command to the remote device */ RFCOMM_SendFrame(RFCOMMChannel->DLCI, false, RFCOMM_Frame_UIH, DataLen, Data, ACLChannel); } RFCOMM_Channel_t* RFCOMM_GetFreeChannelEntry(const uint8_t DLCI) { /* Find a free entry in the RFCOMM channel multiplexer state array */ for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++) { RFCOMM_Channel_t* RFCOMMChannel = &RFCOMM_Channels[i]; /* If the channel's state is closed, the channel state entry is free */ if (RFCOMMChannel->State == RFCOMM_Channel_Closed) { RFCOMMChannel->DLCI = DLCI; RFCOMMChannel->State = RFCOMM_Channel_Configure; RFCOMMChannel->Priority = 7 + (RFCOMMChannel->DLCI & 0xF8); RFCOMMChannel->MTU = 0xFFFF; RFCOMMChannel->Remote.Signals = 0 | (1 << 0); RFCOMMChannel->Remote.BreakSignal = 0 | (1 << 0); RFCOMMChannel->Local.Signals = RFCOMM_SIGNAL_RTC | RFCOMM_SIGNAL_RTR | RFCOMM_SIGNAL_DV | (1 << 0); RFCOMMChannel->Local.BreakSignal = 0 | (1 << 0); RFCOMMChannel->ConfigFlags = 0; return RFCOMMChannel; } } return NULL; } RFCOMM_Channel_t* RFCOMM_GetChannelData(const uint8_t DLCI) { /* Search through the RFCOMM channel list, looking for the specified channel */ for (uint8_t i = 0; i < RFCOMM_MAX_OPEN_CHANNELS; i++) { RFCOMM_Channel_t* CurrRFCOMMChannel = &RFCOMM_Channels[i]; /* If the current non-closed channel's DLCI matches the search DLCI, return it to the caller */ if ((CurrRFCOMMChannel->State != RFCOMM_Channel_Closed) && (CurrRFCOMMChannel->DLCI == DLCI)) return CurrRFCOMMChannel; } /* Channel not found in the channel state table, return failure */ return NULL; } uint16_t RFCOMM_GetVariableFieldValue(const uint8_t** BufferPos) { uint8_t FirstOctet; uint8_t SecondOctet = 0; FirstOctet = **BufferPos; (*BufferPos)++; /* If the field size is more than a single byte, fetch the next byte in the variable length field */ if (!(FirstOctet & 0x01)) { SecondOctet = **BufferPos; (*BufferPos)++; /* Discard any remaining bytes in the variable length field that won't fit in the return value */ while (!(**BufferPos & 0x01)) (*BufferPos)++; } /* Bit-shift the bytes that comprise the variable length field so that they form a single integer */ return (((uint16_t)SecondOctet << 7) | FirstOctet >> 1); } void RFCOMM_SendFrame(const uint8_t DLCI, const bool CommandResponse, const uint8_t Control, const uint16_t DataLen, const void* Data, Bluetooth_Channel_t* const ACLChannel) { struct { RFCOMM_Header_t FrameHeader; uint8_t Size[(DataLen < 128) ? 1 : 2]; uint8_t Data[DataLen]; uint8_t FCS; } ResponsePacket; /* Set the frame header values to the specified address and frame type */ ResponsePacket.FrameHeader.Control = Control; ResponsePacket.FrameHeader.Address = (RFCOMM_Address_t){.DLCI = DLCI, .EA = true, .CR = CommandResponse}; /* Set the lower 7 bits of the packet length */ ResponsePacket.Size[0] = (DataLen << 1); /* Terminate the size field if size is 7 bits or lower, otherwise set the upper 8 bits of the length */ if (DataLen < 128) ResponsePacket.Size[0] |= 0x01; else ResponsePacket.Size[1] = (DataLen >> 7); /* Copy over the packet data from the source buffer to the response packet buffer */ memcpy(ResponsePacket.Data, Data, DataLen); /* Determine the length of the frame which is to be used to calculate the CRC value */ uint8_t CRCLength = sizeof(ResponsePacket.FrameHeader); /* UIH frames do not have the CRC calculated on the Size field in the response, all other frames do */ if ((Control & ~FRAME_POLL_FINAL) != RFCOMM_Frame_UIH) CRCLength += sizeof(ResponsePacket.Size); /* Calculate the frame checksum from the appropriate fields */ ResponsePacket.FCS = RFCOMM_GetFCSValue(&ResponsePacket, CRCLength); /* Send the completed response packet to the sender */ Bluetooth_SendPacket(&ResponsePacket, sizeof(ResponsePacket), ACLChannel); } static uint8_t RFCOMM_GetFCSValue(const void* FrameStart, uint8_t Length) { uint8_t FCS = 0xFF; /* Calculate new Frame CRC value via the given data bytes and the CRC table */ for (uint8_t i = 0; i < Length; i++) FCS = pgm_read_byte(&CRC8_Table[FCS ^ ((const uint8_t*)FrameStart)[i]]); return ~FCS; } static void RFCOMM_ProcessDM(const RFCOMM_Address_t* const FrameAddress, Bluetooth_Channel_t* const ACLChannel) { BT_RFCOMM_DEBUG(1, "<< DM Received"); BT_RFCOMM_DEBUG(2, "-- DLCI 0x%02X", FrameAddress->DLCI); }