feat(srv/nbus2): PoC of the transmit path

Author: iqyx

services/nbus2/nbus2.c

@@ -21,6 +21,11 @@
 #define MODULE_NAME "nbus"
 
 
+uint8_t nbus_my_id[4] = {0, 0, 0, 1};
+uint8_t nbus_my_ep = 0;
+uint16_t nbus_tx_counter = 0;
+
+
 /**
  * tl;dr of the nbus2 packet format
  *
@@ -50,6 +55,19 @@
  */
 
 
+static void marker(uint32_t port, uint32_t pin, bool len) {
+	gpio_set(port, pin);
+	if (len) {
+		vTaskDelay(1);
+	} else {
+		for (int i = 0; i < 1000; i++) {
+			;
+		}
+	}
+	gpio_clear(port, pin);
+}
+
+
 /**
  * @brief Receive exact number of bytes from the input stream
  *
@@ -80,6 +98,7 @@ static nbus_ret_t stream_receive_expect(Nbus *self, uint8_t *buf, size_t size) {
 			return NBUS_RET_TIMEOUT;
 		} else {
 			/* Something wrong, EOF, EOT or whatever. */
+			u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("stream_receive_expect: no-OK %d"), ret);
 			return NBUS_RET_FAILED;
 		}
 	}
@@ -133,7 +152,7 @@ static nbus_ret_t stream_wait_for_eot(Nbus *self) {
 
 
 static nbus_ret_t pbuf_receive_expect(Nbus *self, struct nbus_pbuf *pbuf, size_t len) {
-	if (pbuf->buf_len + len > pbuf->buf_size) {
+	if ((pbuf->buf_len + len) > pbuf->buf_size) {
 		stream_wait_for_eot(self);
 		return NBUS_RET_FAILED;
 	}
@@ -167,85 +186,108 @@ static nbus_ret_t pbuf_receive_expect(Nbus *self, struct nbus_pbuf *pbuf, size_t
  *       Key derivation alone is 80 us but it is done only once.
  *       Chacha20 decryption alone is 38 us.
  */
-static nbus_ret_t receive_process_header(Nbus *self, struct nbus_pbuf *pbuf) {
-	if (pbuf_receive_expect(self, pbuf, 24) != NBUS_RET_OK) {
+static nbus_ret_t nbus_pbuf_receive_header(struct nbus_pbuf *self, Nbus *nbus) {
+	if (pbuf_receive_expect(nbus, self, 24) != NBUS_RET_OK) {
+		//u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("bad header recv"));
 		return NBUS_RET_FAILED;
 	}
+	//u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("ke = 0x%02x 0x%02x"), pbuf->ke[0], pbuf->ke[1]);
 
-	/* 24 bytes received correctly. Decrypt the header (bytes 8-23)
-	 * using the derived key Ke and SIV as a nonce (bytes 0-7). */
-	b2s_derive_keys(self->mac_key, self->mac_key_len, pbuf->ke, pbuf->km);
-	//b2s_crypt(
-		//pbuf->buf + 8, /* Fixed part of the header starts from byte 8, it is 16 bytes long. */
-		//16,
-		//pbuf->buf, /* SIV starts t the beginning, it is always 8 bytees long. */
-		//8,
-		//pbuf->ke
-	//);
-	gpio_set(GPIOA, GPIO15);
-	chacha20_context ctx;
-	chacha20_keysetup(&ctx, pbuf->ke, 128);
-	chacha20_nonce(&ctx, pbuf->buf);
-
-	for (uint32_t counter = 0; counter < 16; counter++) {
-		chacha20_counter(&ctx, counter);
-
-		uint8_t keystream[64];
-		chacha20_keystream(&ctx, keystream);
-		for (size_t i = 0; i < 16; i++) {
-			pbuf->buf[i + 8] ^= keystream[i];
-		}
-	}
-	gpio_clear(GPIOA, GPIO15);
-
-	//u_log(system_log, LOG_TYPE_DEBUG, U_LOG_MODULE_PREFIX("decrypted magic 0x%02x 0x%02x"), pbuf->buf[8], pbuf->buf[9]);
+	/* Decrypt the header. */
+	ChaCha20 ch;
+	chacha20_keysetup(&ch, self->ke, 128);
+	chacha20_nonce(&ch, self->buf);
+	chacha20_encrypt(&ch, self->buf + 8, self->buf + 8, 16);
 
 	/* Check the magic. It is right at the beginning of the fixed header. */
-	if (pbuf->buf[8] != 'n' || pbuf->buf[9] != '2') {
+	if (self->buf[8] != 'n' || self->buf[9] != '2') {
+		u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("bac magic"));
 		return NBUS_RET_FAILED;
 	}
 
-	memcpy(pbuf->dstid, pbuf->buf + 16, 4);
-	memcpy(pbuf->srcid, pbuf->buf + 20, 4);
-
 	return NBUS_RET_OK;
 }
 
 
-static nbus_ret_t receive_process_data(Nbus *self, struct nbus_pbuf *pbuf) {
-	size_t packet_len = pbuf->buf[10] << 8 | pbuf->buf[11];
+static nbus_ret_t nbus_pbuf_receive_data(struct nbus_pbuf *self, Nbus *nbus) {
+	size_t packet_len = self->buf[10] << 8 | self->buf[11];
 	if (packet_len == 0) {
 		/* Zero length packet is valid, do not read anything. */
 		return NBUS_RET_OK;
 	}
-	if (pbuf_receive_expect(self, pbuf, packet_len) != NBUS_RET_OK) {
+	if (pbuf_receive_expect(nbus, self, packet_len) != NBUS_RET_OK) {
 		return NBUS_RET_FAILED;
 	}
-	//u_log(system_log, LOG_TYPE_DEBUG, U_LOG_MODULE_PREFIX("received packet data len %u"), packet_len);
 
+	//u_log(system_log, LOG_TYPE_DEBUG, U_LOG_MODULE_PREFIX("received packet data len %u"), packet_len);
+	//u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("data 0x%02x, 0x%02x, 0x%02x, 0x%02x"), pbuf->buf[24], pbuf->buf[25], pbuf->buf[26], pbuf->buf[27]);
 
-	/* Decrypt data only, headers are already decrypted. It is not possible yet, so do a workaround
-	 * (encrypt header back, decrypt as a whole). */
-	/** @todo This decryption takes 2.16 ms for a 1024 byte packet */
-	b2s_crypt(pbuf->buf + 8, 16, pbuf->buf, 8, pbuf->ke);
-	b2s_crypt(pbuf->buf + 8, pbuf->buf_len - 8, pbuf->buf, 8, pbuf->ke);
-	/** @todo The same decryption using chacha20 should be around 680 us
-	 *        and 570 us when XORing 32 bits at once. */
+	/* Decrypt data only, headers are already decrypted. */
+	ChaCha20 ch;
+	chacha20_keysetup(&ch, self->ke, 128);
+	chacha20_nonce(&ch, self->buf);
+	/* Consume first counter. */
+	uint8_t foo[8] = {0};
+	chacha20_encrypt(&ch, foo, foo, sizeof(foo));
+	chacha20_encrypt(&ch, self->buf + 24, self->buf + 24, packet_len);
 
 	/* Compute MAC from header and data. Compare with the SIV from the message. */
-	/** @todo MAC computation takes 440 us for a 1024 byte packet (blake2s) */
-	//uint8_t mac[BLAKE2S_OUTBYTES];
-	//b2s_siv(pbuf->buf + 8, pbuf->buf_len - 8, mac, sizeof(mac), pbuf->km);
-	/** @todo MAC computation takes 59 us for a 1024 byte packet (halfsiphash) */
 	uint8_t mac[8];
-	halfsiphash(pbuf->buf + 8, pbuf->buf_len - 8, pbuf->km, mac, sizeof(mac));
+	halfsiphash(self->buf + 8, self->buf_len - 8, self->km, mac, sizeof(mac));
+
+	if (memcmp(self->buf, mac, 8)) {
+		//syslog(LOG_ERR, "pbuf: receive data bad MAC");
+		return NBUS_RET_FAILED;
+	}
+
+	return NBUS_RET_OK;
+}
+
 
-	if (memcmp(pbuf->buf, mac, 8)) {
-		//u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("bad MAC 0x%02x 0x%02x != 0x%02x 0x%02x"), pbuf->buf[0], pbuf->buf[1], mac[0], mac[1]);
+static nbus_ret_t nbus_pbuf_send(struct nbus_pbuf *self, void *buf, size_t len, uint8_t dst_id[4], uint8_t dst_ep) {
+	if ((len + 24) > self->buf_size) {
+		return NBUS_RET_FAILED;
+	}
+	memcpy(self->buf + 24, buf, len);
+	self->buf_len = len + 24;
+
+	self->buf[8] = 'n';
+	self->buf[9] = '2';
+
+	self->buf[10] = len / 256;
+	self->buf[11] = len % 256;
+
+	self->buf[12] = nbus_tx_counter / 256;
+	self->buf[13] = nbus_tx_counter % 256;
+	nbus_tx_counter++;
+
+	/* Set IDs and endpoints. */
+	self->buf[14] = (dst_ep & 0xf) << 4 | (nbus_my_ep & 0xf);
+	self->buf[15] = 0;
+	memcpy(self->buf + 16, dst_id, 4);
+	memcpy(self->buf + 20, nbus_my_id, 4);
+
+	return NBUS_RET_OK;
+}
+
+
+static nbus_ret_t nbus_pbuf_transmit(struct nbus_pbuf *self, Nbus *nbus) {
+
+	/* Compute SIV first */
+	halfsiphash(self->buf + 8, self->buf_len - 8, self->km, self->buf, 8);
+
+	ChaCha20 ch;
+	chacha20_keysetup(&ch, self->ke, 128);
+	chacha20_nonce(&ch, self->buf);
+	/* Encrypt the header first. */
+	chacha20_encrypt(&ch, self->buf + 8, self->buf + 8, 16);
+	/* Counter is incremented, the rest of the input is kept. Encrypt the data. */
+	chacha20_encrypt(&ch, self->buf + 24, self->buf + 24, self->buf_len - 24);
+
+	if (nbus->stream->vmt->write(nbus->stream, self->buf, self->buf_len) != STREAM_RET_OK) {
 		return NBUS_RET_FAILED;
 	}
 
-	//b2s_crypt(pbuf->data, pbuf->len, pbuf->siv, 8, self->ke);
 	return NBUS_RET_OK;
 }
 
@@ -254,6 +296,7 @@ static void addr_to_str(uint8_t addr[4], char *s, size_t size) {
 	snprintf(s, size, "0x%02x%02x%02x%02x", addr[0], addr[1], addr[2], addr[3]);
 }
 
+/*
 static void print_pbuf(Nbus *self, struct nbus_pbuf *pbuf, const char *prefix) {
 	(void)self;
 
@@ -264,7 +307,7 @@ static void print_pbuf(Nbus *self, struct nbus_pbuf *pbuf, const char *prefix) {
 
 	u_log(system_log, LOG_TYPE_DEBUG, U_LOG_MODULE_PREFIX("%s%s -> %s, len %u, 0x%02x 0x%02x"), prefix, srcid, dstid, pbuf->buf_len, pbuf->buf[24], pbuf->buf[25]);
 }
-
+*/
 
 static void nbus_mac_task(void *p) {
 	Nbus *self = (Nbus *)p;
@@ -274,40 +317,57 @@ static void nbus_mac_task(void *p) {
 		if (pbuf == NULL) {
 			/* Cannot allocate a buffer, we must drop the packet. Wait at least for EOT. */
 			stream_wait_for_eot(self);
+			u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("buffer allocation error"));
 			goto err;
 		}
-		if (receive_process_header(self, pbuf) != NBUS_RET_OK) {
+
+		if (nbus_pbuf_receive_header(pbuf, self) != NBUS_RET_OK) {
 			stream_wait_for_eot(self);
+			u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("packet header reception error"));
 			goto err;
 		}
+		/* After first 24 bytes */
+		marker(GPIOE, GPIO11, false);
 
-		if (receive_process_data(self, pbuf) != NBUS_RET_OK) {
+		if (nbus_pbuf_receive_data(pbuf, self) != NBUS_RET_OK) {
+			u_log(system_log, LOG_TYPE_WARN, U_LOG_MODULE_PREFIX("packet data reception error"));
 			goto err;
 		}
-
 		//print_pbuf(self, pbuf, "received ");
 
-
-
 		/* End of packet. No additional data should be in the receive buffer. Check for EOT now
 		 * before a new packet reception starts. */
 		if (stream_eot_expect(self) != NBUS_RET_OK) {
 			/* No EOT, additional data received, wait for one. */
 			stream_wait_for_eot(self);
+			//goto err;
 		}
 
-		self->stream->vmt->write(self->stream, "\x00\x55\x00\x55\x00\x55\x00\x55", 8);
+		//self->stream->vmt->write(self->stream, pbuf->buf, pbuf->buf_len);
+		nbus_pbuf_release(self, pbuf);
+
+		struct nbus_pbuf *txp = nbus_pbuf_allocate(self);
+		uint8_t txbuf[256] = {0};
+		uint8_t dstid[4] = {0, 0, 0, 1};
+		nbus_pbuf_send(txp, txbuf, sizeof(txbuf), dstid, 1);
+		nbus_pbuf_transmit(txp, self);
+		nbus_pbuf_release(self, txp);
+		/* Consume echo until EOT. */
+		stream_wait_for_eot(self);
+
+		continue;
 
 err:
 		nbus_pbuf_release(self, pbuf);
-
+		marker(GPIOE, GPIO10, false);
 	}
 	vTaskDelete(NULL);
 }
 
 
 static void nbus_hk_task(void *p) {
 	Nbus *self = (Nbus *)p;
+	(void)self;
 
 	while (true) {
 		vTaskDelay(1000);
@@ -363,6 +423,12 @@ struct nbus_pbuf *nbus_pbuf_allocate(Nbus *self) {
 		if (self->pbufs[i].used == false) {
 			self->pbufs[i].used = true;
 			self->pbufs[i].buf_len = 0;
+			self->pbufs[i].buf_size = NBUS_PBUF_DATA_SIZE;
+			memset(self->pbufs[i].buf, 0, 24);
+
+			/* Prepare keys for the new pbuf */
+			b2s_derive_keys(self->mac_key, self->mac_key_len, self->pbufs[i].ke, self->pbufs[i].km);
+
 			return &(self->pbufs[i]);
 		}
 	}

services/nbus2/nbus2.h

@@ -35,9 +35,6 @@ struct nbus_pbuf {
 	uint8_t ke[B2S_KE_LEN];
 	uint8_t km[B2S_KM_LEN];
 
-	uint8_t dstid[4];
-	uint8_t srcid[4];
-
 	uint8_t *buf;
 	size_t buf_size;
 	size_t buf_len;