motor/Common/spi/SPI_Bridge.c

/*
 * SPI_Bridge.c
 *
 *  Created on: Jun 13, 2020
 *      Author: matth
 */

#include "SPI_Bridge.h"
#include <string.h>

void SPI_Bridge_init(SPI_Bridge_t *b, SPI_DEV_t *hdev,
		osMutexId pack_mutex) {
	b->hdev = hdev;
	b->pack_mutex_id = pack_mutex;
	b->queue_in_hdr = 0;
	b->queue_in_tail = 0;
	b->queue_out_hdr = 0;
	b->queue_out_tail = 0;

	b->stage = 0;

	b->error_cnt = 0;
	b->count_in = 0;
	b->count_out = 0;
	b->pkg_in = 0;
	b->pkg_out = 0;
	b->dia_in = 0; /* diagram in */
	b->dia_out = 0; /* diagram out */
	b->Bps_in = 0;
	b->Pps_in = 0;
	b->Dps_in = 0;
	b->Bps_out = 0;
	b->Pps_out = 0;
	b->Dps_out = 0;
}

void SPI_Bridge_pack(SPI_Bridge_t *b, uint8_t id, uint8_t cmd[], size_t len,
		uint32_t millisec) {
	size_t i;
	uint8_t sum;

	if (osMutexWait(b->pack_mutex_id, millisec) == osOK) {
		b->queue_out[b->queue_out_hdr++] = 0xEF;
		if (b->queue_out_hdr >= SPI_Bridge_QUEUE_LEN)
			b->queue_out_hdr = 0;
		sum = (id << 3) + ((len & 0x0700) >> 8);
		b->queue_out[b->queue_out_hdr++] = sum;
		if (b->queue_out_hdr >= SPI_Bridge_QUEUE_LEN)
			b->queue_out_hdr = 0;

		sum = (len & 0xFF);
		b->queue_out[b->queue_out_hdr++] = sum;
		if (b->queue_out_hdr >= SPI_Bridge_QUEUE_LEN)
			b->queue_out_hdr = 0;

		sum = 0;
		for (i = 0; i < len; ++i) {
			b->queue_out[b->queue_out_hdr++] = cmd[i];
			if (b->queue_out_hdr >= SPI_Bridge_QUEUE_LEN)
				b->queue_out_hdr = 0;

			sum ^= cmd[i];
		}

		b->queue_out[b->queue_out_hdr++] = sum;
		if (b->queue_out_hdr >= SPI_Bridge_QUEUE_LEN)
			b->queue_out_hdr = 0;

		b->count_out += len;
		b->pkg_out += 1;
		osMutexRelease(b->pack_mutex_id);
	}
}

int SPI_Bridge_unpack(SPI_Bridge_t *b) {
	uint8_t c;
	int ret;

	ret = -1;

	if (b->queue_in_hdr != b->queue_in_tail) {
		c = b->queue_in[b->queue_in_tail++];
		if (b->queue_in_tail >= SPI_Bridge_QUEUE_LEN)
			b->queue_in_tail = 0;
		switch (b->stage) {
		case 0:
			if (c == 0xEF) {
				++b->stage;
			}
			break;
		case 1:
			b->id = c >> 3;
			b->len = (c & 0x7) * 256;
			++b->stage;
			break;
		case 2:
			b->len += c;
			b->sum = 0;
			b->idx = 0;
			if (b->len > SPI_Bridge_UNPACK_LEN) {
				b->stage = 0;
				b->error_cnt++;
			} else {
				++b->stage;
			}
			break;
		case 3:
			if (b->idx < b->len) {
				b->data[b->idx] = c;
				b->sum ^= c;
				++b->idx;
			} else {
				if (b->sum == c) {
					ret = b->id;
					b->count_in += b->len;
					b->pkg_in += 1;
				}
				else
				{
					b->error_cnt++;
				}
				b->stage = 0;
			}
			break;
		default:
			b->stage = 0;
		}
	} else {
		ret = -2;
	}
	return ret;

}

void SPI_Bridge_Slave_TransmitReceive(SPI_Bridge_t *b, uint32_t millisec, uint32_t millisec_trans) {
	int i;

	if (!SPI_DEV_begin(b->hdev, millisec))
	{
		b->error_cnt++;
		return;
	}

	b->buff[0] = 0xEB;
	b->buff[1] = 0x90;
	for (i = 2; i < SPI_Bridge_BLOCK_LEN; ++i) {

		if (b->queue_out_hdr != b->queue_out_tail) {
			b->buff[i] = b->queue_out[b->queue_out_tail++];
			if (b->queue_out_tail >= SPI_Bridge_QUEUE_LEN)
				b->queue_out_tail = 0;
		} else {
			b->buff[i] = 0xFE;
		}
	}
	b->dia_out++;

	if (SPI_DEV_transfer(b->hdev, b->buff, SPI_Bridge_BLOCK_LEN,
			b->buff_in,	SPI_Bridge_BLOCK_LEN, millisec_trans))
	{
		if (b->buff_in[0] == 0xEB && b->buff_in[1] == 0x90) {
			for (i = 2; i < SPI_Bridge_BLOCK_LEN; ++i) {
				b->queue_in[b->queue_in_hdr++] = b->buff_in[i];
				if (b->queue_in_hdr >= SPI_Bridge_QUEUE_LEN)
					b->queue_in_hdr = 0;
			}
			b->dia_in++;
		}
		else
		{
			b->error_cnt++;
		}
	}
	else
	{
		b->error_cnt++;
	}

	SPI_DEV_end(b->hdev);
}


void SPI_Bridge_Master_TransmitReceive(SPI_Bridge_t *b, uint32_t millisec, uint32_t millisec_wait, uint32_t millisec_trans) {
	int i;

	if (!SPI_DEV_begin(b->hdev, millisec))
	{
		b->error_cnt++;
		return;
	}

	SPI_DEV_select(b->hdev);

	b->buff[0] = 0xEB;
	b->buff[1] = 0x90;
	for (i = 2; i < SPI_Bridge_BLOCK_LEN; ++i) {

		if (b->queue_out_hdr != b->queue_out_tail) {
			b->buff[i] = b->queue_out[b->queue_out_tail++];
			if (b->queue_out_tail >= SPI_Bridge_QUEUE_LEN)
				b->queue_out_tail = 0;
		} else {
			b->buff[i] = 0xFE;
		}
	}
	b->dia_out++;

	osDelay(millisec_wait);

	if (SPI_DEV_transfer(b->hdev, b->buff, SPI_Bridge_BLOCK_LEN,
			b->buff_in,	SPI_Bridge_BLOCK_LEN,millisec_trans))
	{
		if (b->buff_in[0] == 0xEB && b->buff_in[1] == 0x90) {
			for (i = 2; i < SPI_Bridge_BLOCK_LEN; ++i) {
				b->queue_in[b->queue_in_hdr++] = b->buff_in[i];
				if (b->queue_in_hdr >= SPI_Bridge_QUEUE_LEN)
					b->queue_in_hdr = 0;
			}
			b->dia_in++;
		}
		else
		{
			b->error_cnt++;
		}
	}
	else
	{
		b->error_cnt++;
	}

	SPI_DEV_unselect(b->hdev);

	SPI_DEV_end(b->hdev);
}

void SPI_Bridge_stats(SPI_Bridge_t *b)
{
	b->Bps_in = b->count_in;
	b->Pps_in = b->pkg_in;
	b->Dps_in = b->dia_in;
	b->Bps_out = b->count_out;
	b->Pps_out = b->pkg_out;
	b->Dps_out = b->dia_out;

	b->count_in = 0;
	b->count_out = 0;
	b->pkg_in = 0;
	b->pkg_out = 0;
	b->dia_in = 0; /* diagram in */
	b->dia_out = 0; /* diagram out */
}