274 lines
7.7 KiB
C
274 lines
7.7 KiB
C
#include <string.h>
|
|
#include "main.h"
|
|
#include "worker.h"
|
|
|
|
#include "hdlc.h"
|
|
#include "fifo.h"
|
|
|
|
extern void WORK_formattedPrintArray(char* str, uint8_t* array, size_t size);
|
|
|
|
workerHost_s* WORK_workerInit (workerHost_s* worker,
|
|
hdlcPacket_s* packet, fifo_s* fifo, uint8_t* buffer) {
|
|
if (!worker->isInitialized) {
|
|
printf("WORKX: Starting master worker initialization\n");
|
|
worker->packet = packet;
|
|
worker->fifoWrite = fifo;
|
|
worker->fifoRead = NULL;
|
|
worker->isInitialized = true;
|
|
worker->byteBuffer = buffer;
|
|
|
|
FIFO_init(worker->fifoWrite);
|
|
|
|
printf("WORKX: Master initialization done\n");
|
|
WORK_printBlocks(worker, 3);
|
|
|
|
return worker;
|
|
}
|
|
|
|
// Ошибка в пакете, повторная инициализация
|
|
worker->isInitialized = false;
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
int WORK_waitClient (fifo_s *clientFifo) {
|
|
printf("WORKX: .. Waiting for a client\n");
|
|
if (FIFO_isUsable(clientFifo) == 0) {
|
|
return 0;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
|
|
// Подготовка пакета для отправки
|
|
int WORK_preparePackage(workerHost_s *worker) {
|
|
printf("WORKX: .. Preparing package\n");
|
|
|
|
// Перемещение структуры в массив на передачу
|
|
// Вычитаем байты начала и конца сообщения
|
|
size_t structSize = HDLC_PACKET_LENGTH-2;
|
|
// size_t structSize = sizeof(hdlcPacket_s);
|
|
|
|
uint8_t tmpPtr = 0;
|
|
uint8_t *packPtr = (uint8_t*)worker->packet;
|
|
|
|
worker->byteBuffer[tmpPtr++] = HDLC_PACKET_FLAG;
|
|
for (size_t s = 0; s < structSize; s++, tmpPtr++) {
|
|
// Нашли флаг в теле сообщения
|
|
if (packPtr[s] == HDLC_PACKET_FLAG) {
|
|
worker->byteBuffer[tmpPtr] = HDLC_PACKET_ESCAPE;
|
|
tmpPtr++;
|
|
}
|
|
worker->byteBuffer[tmpPtr] = packPtr[s];
|
|
}
|
|
|
|
worker->byteBuffer[tmpPtr] = HDLC_PACKET_FLAG;
|
|
worker->byteBufferLength = tmpPtr+1;
|
|
WORK_formattedPrintArray("prepared bytes:",
|
|
worker->byteBuffer, worker->byteBufferLength);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// Отправка пакета
|
|
int WORK_sendPackage(workerHost_s *worker) {
|
|
printf("WORKX: .. Sending package\n");
|
|
|
|
if (worker->byteBufferHead > worker->byteBufferLength) {
|
|
return -2;
|
|
}
|
|
|
|
if (FIFO_isUsable(worker->fifoWrite) == 0) {
|
|
if (worker->fifoWrite->count < FIFO_DATA_SIZE) {
|
|
FIFO_put(worker->fifoWrite, worker->byteBuffer[worker->byteBufferHead]);
|
|
if (worker->byteBufferHead < worker->byteBufferLength) {
|
|
worker->byteBufferHead++;
|
|
} else if (worker->byteBufferHead == worker->byteBufferLength) {
|
|
// Передан весь буфер через фифо
|
|
WORK_formattedPrintArray("sent bytes:",
|
|
worker->byteBuffer, worker->byteBufferLength);
|
|
return 0;
|
|
} else {
|
|
worker->byteBufferHead = 0;
|
|
return -2;
|
|
}
|
|
} else {
|
|
return 1;
|
|
}
|
|
return -1;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
|
|
// Ожидание пакета
|
|
int WORK_waitPackage(workerHost_s *worker) {
|
|
printf("WORKX: .. Waiting for a package\n");
|
|
if (FIFO_isUsable(worker->fifoRead) == 0) {
|
|
if (worker->fifoRead->count > 0) {
|
|
// Появились данные в fifo
|
|
return 0;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int WORK_receivePackage(workerHost_s *worker) {
|
|
printf("WORKX: .. Receiving package\n");
|
|
if (FIFO_isUsable(worker->fifoRead) == 0) {
|
|
if (worker->fifoRead->count > 0) {
|
|
FIFO_get(worker->fifoRead, &worker->byteBuffer[worker->byteBufferHead]);
|
|
if (worker->byteBufferHead < WORK_BYTE_BUFFER_SIZE) {
|
|
worker->byteBufferHead++;
|
|
} else {
|
|
worker->byteBufferHead = 0;
|
|
return -2;
|
|
}
|
|
} else {
|
|
if (worker->byteBufferHead > 0) {
|
|
// Приняли данные. Сейчас fifo пуст, можно обрабатывать пакет
|
|
// fifo пуст, можно обрабатывать пакет
|
|
WORK_formattedPrintArray("receives bytes:",
|
|
worker->byteBuffer, worker->byteBufferLength);
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
|
|
int WORK_parsePackage(workerHost_s *worker) {
|
|
printf("WORKX: .. Parsing package\n");
|
|
|
|
// Поиск пакета в массиве
|
|
uint8_t *first = NULL, *last = NULL;
|
|
size_t bufPtrI = 0, bufLen = 0;
|
|
uint8_t tmpArr[WORK_BYTE_BUFFER_SIZE] = {0};
|
|
// Выносим из принятого массива только нагрузку в буфер
|
|
for (bufPtrI = 0; bufPtrI < WORK_BYTE_BUFFER_SIZE; bufPtrI++) {
|
|
// Найден флаг
|
|
if (worker->byteBuffer[bufPtrI] == HDLC_PACKET_FLAG) {
|
|
if (!first) {
|
|
first = &worker->byteBuffer[bufPtrI];
|
|
first++; // Скипаем байт флага
|
|
} else if (!last) {
|
|
last = &worker->byteBuffer[bufPtrI];
|
|
last--;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Найден escape-флаг
|
|
if (worker->byteBuffer[bufPtrI] == HDLC_PACKET_ESCAPE) {
|
|
if (worker->byteBuffer[bufPtrI+1] == HDLC_PACKET_FLAG) {
|
|
bufPtrI++;
|
|
}
|
|
}
|
|
}
|
|
|
|
WORK_formattedPrintArray("received bytes:", first, bufPtrI);
|
|
|
|
// Разбор пакета в структуру
|
|
if (first && last) {
|
|
if (last > first) {
|
|
bufLen = last - first;
|
|
// Так как может быть меньше в теории
|
|
if (bufLen <= HDLC_PACKET_LENGTH-2) {
|
|
if (bufLen > 2) { // Считываем адрес
|
|
uint16_t a = ((uint16_t)*first << 8) | *(first+1);
|
|
worker->packet->address = ((uint16_t)*(first+1) << 8) | *(first);
|
|
first += 2;
|
|
}
|
|
if (bufLen > 4) { // Считываем управляющее поле
|
|
worker->packet->control = ((uint16_t)*(first+1) << 8) | *(first);
|
|
first += 2;
|
|
}
|
|
if (bufLen > 6) { // Считываем контрольную сумму
|
|
worker->packet->fcs = ((uint16_t)*(last) << 8) | *(last-1);
|
|
last -= 2;
|
|
}
|
|
if ((bufLen > 8) && (last > first)) { // Считываем информационный пакет
|
|
int j = 0;
|
|
// От переполнения
|
|
size_t infoLen = last - first;
|
|
if (infoLen > HDLC_INFO_LENGTH) {
|
|
return -1;
|
|
}
|
|
|
|
// Заполнение данными
|
|
for (uint8_t* i = first; i < last; i++) {
|
|
worker->packet->info[j] = *i;
|
|
j++;
|
|
}
|
|
|
|
// Чистка хвоста
|
|
for (; j < HDLC_INFO_LENGTH; j++) {
|
|
worker->packet->info[j] = 0;
|
|
}
|
|
}
|
|
}
|
|
} // (last > first)
|
|
} // (first && last)
|
|
else {
|
|
// неправильный пакет
|
|
return -1;
|
|
}
|
|
|
|
WORK_formattedPrintArray("parsed bytes:", worker->byteBuffer, bufPtrI);
|
|
|
|
|
|
// Проверка контрольной суммы разобранного пакета
|
|
uint16_t crc = worker->packet->fcs;
|
|
worker->packet->fcs = 0;
|
|
if (CRC16_compute((uint8_t*) &worker->packet, sizeof(workerHost_s)) == crc) {
|
|
printf("WORKX: .. CRC OK");
|
|
return 0;
|
|
} else {
|
|
printf("WORKX: .. WRONG CRC");
|
|
return -1;
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
|
|
int WORK_checkPackage(workerHost_s *worker) {
|
|
printf("WORKX: .. Checking package\n");
|
|
int res = 0;
|
|
return 0;
|
|
res = HDLC_isControlFieldValid(worker->packet->control);
|
|
// Проверка поля управления
|
|
if (res != 0) {
|
|
return res;
|
|
}
|
|
printf("WORKX: .. Package is info ...\n");
|
|
|
|
// Проверка контрольной суммы
|
|
|
|
printf("WORKX: .. Package checked\n");
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// Вызов функций обработчика хоста
|
|
int WORK_execute(int (*callback)(workerHost_s*), workerHost_s *worker) {
|
|
if (callback == NULL) {
|
|
printf("WORKX: .. Wrong object\n");
|
|
return -1;
|
|
}
|
|
|
|
int res = callback(worker);
|
|
|
|
if (res != 0) {
|
|
printf("WORKX: .. Error\n");
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|