/*
 * mki7.c
 *
 *  Created on: 20-03-2020
 *      Author: Krzysztof Jakubczyk
 */

#include "tdefs.h"
#include "misc.h"

#include "mki7.h"
#include "helper.h"

volatile u8 mki7_sfplinks = 0xFF;


int mki7_initlog(void *arguments, void *logic)
{
	struct mki7_args *args = (struct mki7_args *)arguments;
	struct mki7_logic *log = (struct mki7_logic *)logic;

	if(set_bit_ptr_struct(args->io.z141_out,&log->Z141))
		return -1;
	if(set_bit_ptr_struct(args->io.z142_out,&log->Z142))
		return -1;
	if(set_bit_ptr_struct(args->io.z143_out,&log->Z143))
		return -1;
	if(set_bit_ptr_struct(args->io.z144_out,&log->Z144))
		return -1;
	if(set_bit_ptr_struct(args->io.err_out,&log->err))
		return -1;

	return 0;
}

void mki7_20hz(void *arguments, void *logic)
{
	struct mki7_logic *log = (struct mki7_logic *)logic;

	if(mki7_sfplinks == 0xFF)
	{
		set_struct(&log->err);
		clear_struct(&log->Z141);
		clear_struct(&log->Z142);
		clear_struct(&log->Z143);
		clear_struct(&log->Z144);
	}
	else
	{
		clear_struct(&log->err);
		check_and_set_struct(mki7_sfplinks & 0x01,&log->Z141);
		check_and_set_struct(mki7_sfplinks & 0x02,&log->Z142);
		check_and_set_struct(mki7_sfplinks & 0x04,&log->Z143);
		check_and_set_struct(mki7_sfplinks & 0x08,&log->Z144);
	}
}
