bluez5: implement BLE midi data writer

spa/plugins/bluez5/midi-parser.c

@@ -203,3 +203,93 @@ malformed:
 	/* Error (potentially recoverable) */
 	return -EINVAL;
 }
+
+
+int spa_bt_midi_writer_write(struct spa_bt_midi_writer *writer,
+		uint64_t time, const uint8_t *event, size_t event_size)
+{
+	/* BLE MIDI-1.0: maximum payload size is MTU - 3 */
+	const unsigned int max_size = writer->mtu - 3;
+	const uint64_t time_msec = (time / SPA_NSEC_PER_MSEC);
+	const uint16_t timestamp = time_msec & 0x1fff;
+
+#define PUT(byte) do { if (writer->size >= max_size) return -ENOSPC; \
+		writer->buf[writer->size++] = (byte); } while (0)
+
+	if (writer->mtu < 5+3)
+		return -ENOSPC;  /* all events must fit */
+
+	spa_assert(max_size <= sizeof(writer->buf));
+	spa_assert(writer->size <= max_size);
+
+	if (event_size == 0)
+		return 0;
+
+	if (writer->flush) {
+		writer->flush = false;
+		writer->size = 0;
+	}
+
+	if (writer->size == max_size)
+		goto flush;
+
+	/* Packet header */
+	if (writer->size == 0) {
+		PUT(0x80 | (timestamp >> 7));
+		writer->running_status = 0;
+		writer->running_time_msec = time_msec;
+	}
+
+	/* Timestamp low bits can wrap around, but not multiple times */
+	if (time_msec > writer->running_time_msec + 0x7f)
+		goto flush;
+
+	spa_assert(writer->pos < event_size);
+
+	for (; writer->pos < event_size; ++writer->pos) {
+		const unsigned int unused = max_size - writer->size;
+		const uint8_t byte = event[writer->pos];
+
+		if (byte & 0x80) {
+			enum midi_event_class class;
+			unsigned int expected_size;
+
+			class = midi_event_info(event[0], &expected_size);
+
+			if (class == MIDI_BASIC && expected_size > 1 &&
+					writer->running_status == byte &&
+					writer->running_time_msec == time_msec) {
+				/* Running status: continue with data */
+				continue;
+			}
+
+			if (unused < expected_size + 1)
+				goto flush;
+
+			/* Timestamp before status */
+			PUT(0x80 | (timestamp & 0x7f));
+			writer->running_time_msec = time_msec;
+
+			if (class == MIDI_BASIC && expected_size > 1)
+				writer->running_status = byte;
+			else
+				writer->running_status = 0;
+		} else if (unused == 0) {
+			break;
+		}
+
+		PUT(byte);
+	}
+
+	if (writer->pos < event_size)
+		goto flush;
+
+	writer->pos = 0;
+	return 0;
+
+flush:
+	writer->flush = true;
+	return 1;
+
+#undef PUT
+}

spa/plugins/bluez5/midi.h

@@ -42,7 +42,11 @@
 #define BT_MIDI_SERVICE_UUID		"03b80e5a-ede8-4b33-a751-6ce34ec4c700"
 #define BT_MIDI_CHR_UUID		"7772e5db-3868-4112-a1a9-f2669d106bf3"
 
-#define MIDI_BUF_SIZE	8192
+#define MIDI_BUF_SIZE		8192
+#define MIDI_MAX_MTU		8192
+
+#define MIDI_CLOCK_PERIOD_MSEC	0x2000
+#define MIDI_CLOCK_PERIOD_NSEC	(MIDI_CLOCK_PERIOD_MSEC * SPA_NSEC_PER_MSEC)
 
 struct spa_bt_midi_parser {
 	unsigned int size;
@@ -50,6 +54,16 @@ struct spa_bt_midi_parser {
 	uint8_t buf[MIDI_BUF_SIZE];
 };
 
+struct spa_bt_midi_writer {
+	unsigned int size;
+	unsigned int mtu;
+	unsigned int pos;
+	uint8_t running_status;
+	uint64_t running_time_msec;
+	unsigned int flush:1;
+	uint8_t buf[MIDI_MAX_MTU];
+};
+
 static inline void spa_bt_midi_parser_init(struct spa_bt_midi_parser *parser)
 {
 	parser->size = 0;
@@ -73,4 +87,34 @@ int spa_bt_midi_parser_parse(struct spa_bt_midi_parser *parser,
 		void (*event)(void *user_data, uint16_t time, uint8_t *event, size_t event_size),
 		void *user_data);
 
+static inline void spa_bt_midi_writer_init(struct spa_bt_midi_writer *writer, unsigned int mtu)
+{
+	writer->size = 0;
+	writer->mtu = SPA_MIN(mtu, (unsigned int)MIDI_MAX_MTU);
+	writer->pos = 0;
+	writer->running_status = 0;
+	writer->running_time_msec = 0;
+	writer->flush = 0;
+}
+
+/**
+ * Add a new event to midi writer buffer.
+ *
+ * spa_bt_midi_writer_init(&writer, mtu);
+ * for (time, event, size) in midi events {
+ *     do {
+ *         res = spa_bt_midi_writer_write(&writer, time, event, size);
+ *         if (res < 0) {
+ *             fail with error
+ *         } else if (res) {
+ *             send_packet(writer->buf, writer->size);
+ *         }
+ *     } while (res);
+ * }
+ * if (writer.size > 0)
+ *     send_packet(writer->buf, writer->size);
+ */
+int spa_bt_midi_writer_write(struct spa_bt_midi_writer *writer,
+		uint64_t time, const uint8_t *event, size_t event_size);
+
 #endif