bluez5: add HFP codecs in the media codec API

Add copy of HFP codec implementations in the media codec API.

spa/plugins/bluez5/hfp-h2.h

@@ -0,0 +1,128 @@
+/* Spa HFP Codecs */
+/* SPDX-FileCopyrightText: Copyright © 2025 Pauli Virtanen */
+/* SPDX-License-Identifier: MIT */
+
+#ifndef SPA_BLUEZ5_HFP_H2_H
+#define SPA_BLUEZ5_HFP_H2_H
+
+#define H2_PACKET_SIZE		60
+
+struct h2_reader {
+	uint8_t buf[H2_PACKET_SIZE];
+	uint8_t pos;
+	bool msbc;
+	uint16_t seq;
+	bool started;
+};
+
+struct h2_writer {
+	uint8_t seq;
+};
+
+static inline void h2_reader_init(struct h2_reader *this, bool msbc)
+{
+	this->pos = 0;
+	this->msbc = msbc;
+	this->seq = 0;
+	this->started = false;
+}
+
+static inline void h2_reader_append_byte(struct h2_reader *this, uint8_t byte)
+{
+        /* Parse H2 sync header */
+        if (this->pos == 0) {
+                if (byte != 0x01) {
+                        this->pos = 0;
+                        return;
+                }
+        } else if (this->pos == 1) {
+                if (!((byte & 0x0F) == 0x08 &&
+                      ((byte >> 4) & 1) == ((byte >> 5) & 1) &&
+                      ((byte >> 6) & 1) == ((byte >> 7) & 1))) {
+                        this->pos = 0;
+                        return;
+                }
+        } else if (this->msbc) {
+		/* Beginning of MSBC frame: SYNCWORD + 2 nul bytes */
+		if (this->pos == 2) {
+			if (byte != 0xAD) {
+				this->pos = 0;
+				return;
+			}
+		}
+		else if (this->pos == 3) {
+			if (byte != 0x00) {
+				this->pos = 0;
+				return;
+			}
+		}
+		else if (this->pos == 4) {
+			if (byte != 0x00) {
+				this->pos = 0;
+				return;
+			}
+                }
+	}
+
+	if (this->pos >= H2_PACKET_SIZE) {
+		/* Packet completed. Reset. */
+		this->pos = 0;
+		h2_reader_append_byte(this, byte);
+		return;
+	}
+
+	this->buf[this->pos] = byte;
+	++this->pos;
+}
+
+static inline void *h2_reader_read(struct h2_reader *this, const uint8_t *src, size_t src_size, size_t *consumed, size_t *avail)
+{
+	int seq;
+	size_t i;
+
+	for (i = 0; i < src_size && this->pos < H2_PACKET_SIZE; ++i)
+		h2_reader_append_byte(this, src[i]);
+
+	*consumed = i;
+	*avail = 0;
+
+	if (this->pos < H2_PACKET_SIZE)
+		return NULL;
+
+	this->pos = 0;
+
+	seq = ((this->buf[1] >> 4) & 1) | ((this->buf[1] >> 6) & 2);
+	if (!this->started) {
+		this->seq = seq;
+		this->started = true;
+	}
+
+	this->seq++;
+	while (seq != this->seq % 4)
+		this->seq++;
+
+	*avail = H2_PACKET_SIZE - 2;
+	return &this->buf[2];
+}
+
+static inline void h2_write(uint8_t *buf, uint8_t seq)
+{
+	static const uint8_t sntable[4] = { 0x08, 0x38, 0xc8, 0xf8 };
+
+	buf[0] = 0x01;
+	buf[1] = sntable[seq % 4];
+	buf[59] = 0;
+}
+
+static inline bool is_zero_packet(const uint8_t *data, size_t size)
+{
+	size_t i;
+
+	for (i = 0; i < size; ++i)
+		if (data[i])
+			return false;
+
+	return true;
+}
+
+#endif