/* tej - changed return type to void as nothing returned */
static void InitMFS(void)
{
FAR_PTR LOL;
FAR_PTR SDA;
unsigned char _osmajor;
unsigned char _osminor;
state_t preg;
LOL = GetListOfLists();
SDA = GetSDAPointer();
/* now get the DOS version */
pre_msdos();
HI(ax) = 0x30;
call_msdos();
_osmajor = LO(ax);
_osminor = HI(ax);
post_msdos();
/* get DOS version into CX */
preg.ecx = _osmajor | (_osminor <<8);
preg.edx = FP_OFF16(LOL);
preg.es = FP_SEG16(LOL);
preg.esi = FP_OFF16(SDA);
preg.ds = FP_SEG16(SDA);
preg.ebx = 0x500;
mfs_helper(&preg);
}
/** \brief Software SPI send.
*
* Software SPI implementation to communicate with
* RFM12B module.
*
* \param cmd uint16_t to send to RFM12B
*
*/
unsigned int rf12_send_command(unsigned int cmd)
{
unsigned char i;
unsigned int recv;
recv = 0;
LO(SCK);
LO(CS);
for(i=0; i<16; i++)
{
if(cmd&0x8000)
{
HI(SDI);
}
else
{
LO(SDI);
}
HI(SCK);
recv = recv << 1;
if( RF_PIN&(1<<SDO) )
{
recv|=0x0001;
}
LO(SCK);
cmd = cmd << 1;
}
HI(CS);
return recv;
}
static int make_period_modifier(struct iforce* iforce,
struct resource* mod_chunk, int no_alloc,
__s16 magnitude, __s16 offset, u16 period, u16 phase)
{
unsigned char data[7];
period = TIME_SCALE(period);
if (!no_alloc) {
mutex_lock(&iforce->mem_mutex);
if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0c,
iforce->device_memory.start, iforce->device_memory.end, 2L,
NULL, NULL)) {
mutex_unlock(&iforce->mem_mutex);
return -ENOSPC;
}
mutex_unlock(&iforce->mem_mutex);
}
data[0] = LO(mod_chunk->start);
data[1] = HI(mod_chunk->start);
data[2] = HIFIX80(magnitude);
data[3] = HIFIX80(offset);
data[4] = HI(phase);
data[5] = LO(period);
data[6] = HI(period);
iforce_send_packet(iforce, FF_CMD_PERIOD, data);
return 0;
}
static void
mpdigmul(mpdigit a, mpdigit b, mpdigit *p)
{
mpdigit x, ah, al, bh, bl, p1, p2, p3, p4;
int carry;
/* half digits */
ah = HI(a);
al = LO(a);
bh = HI(b);
bl = LO(b);
/* partial products */
p1 = ah*bl;
p2 = bh*al;
p3 = bl*al;
p4 = ah*bh;
/* p = ((p1+p2)<<(Dbits/2)) + (p4<<Dbits) + p3 */
carry = 0;
x = p1<<(Dbits/2);
p3 += x;
if(p3 < x)
carry++;
x = p2<<(Dbits/2);
p3 += x;
if(p3 < x)
carry++;
p4 += carry + HI(p1) + HI(p2); /* can't carry out of the high digit */
p[0] = p3;
p[1] = p4;
}
int stlink2_swim_read_range(programmer_t *pgm, stm8_device_t *device, unsigned char *buffer, unsigned int start, unsigned int length) {
stlink2_init_session(pgm);
int i;
for(i = 0; i < length; i += STLK_READ_BUFFER_SIZE) {
// Determining current block start & size (relative to 0x8000)
int block_start = start + i;
int block_size = length - i;
if(block_size > STLK_READ_BUFFER_SIZE) {
block_size = STLK_READ_BUFFER_SIZE;
}
// Sending USB packet
stlink2_cmd(pgm, 0xf40b, 6,
HI(block_size), LO(block_size),
0x00, 0x00,
HI(block_start), LO(block_start));
TRY(128, (stlink2_get_status(pgm) & 0xffff) == 0);
// Seems like we've got some bytes from stlink, downloading them
stlink2_cmd(pgm, 0xf40c, 0);
msg_recv(pgm, &(buffer[i]), block_size);
}
return(length);
}
void eeprom_init(void)
{ char j, sofs, i;
char *ptr;
unsigned short checksum;
eeprom[0]= 0x00;
eeprom[1]= 0x00;
eeprom[2]= LO(dev_descript.idVendor);
eeprom[3]= HI(dev_descript.idVendor);
eeprom[4]= LO(dev_descript.idProduct);
eeprom[5]= HI(dev_descript.idProduct);
eeprom[6]= LO(dev_descript.bcdDevice);
eeprom[7]= HI(dev_descript.bcdDevice);
eeprom[8]= config_descript.bmAttributes;
eeprom[9] = config_descript.MaxPower;
eeprom[10] = 0x1C;
eeprom[11] = 0x00;
eeprom[12] = LO(dev_descript.bcdUSB);
eeprom[13] = HI(dev_descript.bcdUSB);
eeprom[14] = sofs = 0x86+14;
sofs += (eeprom[15] = LO(ManufacturerDscr[0]));
eeprom[16]= sofs;
sofs += (eeprom[17]=ProductDscr[0]);
eeprom[18]= sofs;
eeprom[19]= LO(SerialNumDscr[0]);
i=20;
for(j=0, ptr = (char*) &(ManufacturerDscr);j<LO(ManufacturerDscr[0]);j++)
eeprom[i++]=ptr[j];
for(j=0, ptr = (char*) &(ProductDscr);j<LO(ProductDscr[0]);j++)
eeprom[i++]=ptr[j];
for(j=0, ptr = (char*) &(SerialNumDscr);j<LO(SerialNumDscr[0]);j++)
eeprom[i++]=ptr[j];
for(j=0, ptr = (char*) & dev_descript.iManufacturer;j<4;j++)
eeprom[i++]=ptr[j];
while(i<126)
eeprom[i++]=0;
checksum = 0xAAAA;
for(i=0;i<63;i++)
{ checksum^=((word*)eeprom)[i];
if(checksum&0x8000)
checksum=(checksum<<1)|1;
else
checksum=(checksum<<1);
}
((word*)eeprom)[63]=checksum;
}
int stlink2_write_word(programmer_t *pgm, unsigned int word, unsigned int start) {
unsigned char buf[4], start2[2];
pack_int16(start, start2);
stlink2_cmd(pgm, 0xf40a, 8,
0x00, 0x02,
0x00, 0x00,
HI(start), LO(start),
HI(word), LO(word));
usleep(2000);
return(stlink2_get_status(pgm)); // Should be '1'
}
/** \brief Setup the AVR ports used by the Module
*
* Set up the correct data direction and state for the
* ports used by the RFM12B as specifed in rfm12b.h
*/
void rf12_portSetup()
{
RF_PORT = 0x0;
HI(CS);
HI(SDI);
LO(SCK);
RF_DDR = (1<<CS) | (1<<SDI) | (1<<SCK);
HI(NIRQ);
HI(CS);
LO(SDI);
HI(SDO);
}
INT16U rtu_getAnalogInput(INT8U addr,INT8U *buf,INT16U start_address,INT16U lenth)
{
unsigned char tmp[256],tmp_lenth;
tmp[0] = addr;
tmp[1] = READ_AI;
tmp[2] = HI(start_address);
tmp[3] = LOW(start_address);
tmp[4] = HI(lenth);
tmp[5] = LOW(lenth);
tmp_lenth = 6;
construct_rtu_frm ( buf,tmp,tmp_lenth);
return 8;
}
void usart_hex(unsigned char c){
#define HI(b) ((b>>4) & 0xf)
#define LO(b) (b & 0xf)
#define HE(b) (((b & 0x7)-1) | 0x40)
if (HI(c)>9)
usart_putchar(HE(HI(c)));
else
usart_putchar(0x30 | HI(c));
if (LO(c)>9)
usart_putchar(HE(LO(c)));
else
usart_putchar(0x30 | LO(c));
}
INT16U rtu_setHoldingRegister(INT8U addr,INT8U *buf, INT16U start_address,INT16U value )
{
unsigned char tmp[256], tmp_lenth;
tmp[0] = addr;
tmp[1] = SET_HLD_REG;
tmp[2] = HI(start_address);
tmp[3] = LOW(start_address);
tmp[4] = HI(value);
tmp[5] = LOW(value);
tmp_lenth = 6;
construct_rtu_frm ( buf, tmp, tmp_lenth);
return 8 ;
}
u08 i2c_eep_WriteByte(u08 SAddr,u16 Addr, u08 Byte, IIC_F WhatDo)
{
if (i2c_Do & i2c_Busy) return 0;
i2c_index = 0;
i2c_ByteCount = 3;
i2c_SlaveAddress = SAddr;
i2c_Buffer[0] = HI(Addr);
i2c_Buffer[1] = LO(Addr);
i2c_Buffer[2] = Byte;
i2c_Do = i2c_sawp;
MasterOutFunc = WhatDo;
ErrorOutFunc = WhatDo;
TWCR = 1<<TWSTA|0<<TWSTO|1<<TWINT|0<<TWEA|1<<TWEN|1<<TWIE;
i2c_Do |= i2c_Busy;
return 1;
}
u08 i2c_eep_ReadByte(u08 SAddr, u16 Addr, u08 ByteNumber, IIC_F WhatDo)
{
if (i2c_Do & i2c_Busy) return 0;
i2c_index = 0;
i2c_ByteCount = ByteNumber;
i2c_SlaveAddress = SAddr;
i2c_PageAddress[0] = HI(Addr);
i2c_PageAddress[1] = LO(Addr);
i2c_PageAddrIndex = 0;
i2c_PageAddrCount = 2;
i2c_Do = i2c_sawsarp;
MasterOutFunc = WhatDo;
ErrorOutFunc = WhatDo;
TWCR = 1<<TWSTA|0<<TWSTO|1<<TWINT|0<<TWEA|1<<TWEN|1<<TWIE;
i2c_Do |= i2c_Busy;
return 1;
}
void xms_control(void)
{
int is_umb_fn = 0;
/* First do the UMB functions */
switch (HI(ax)) {
case XMS_ALLOCATE_UMB:
{
int size = LWORD(edx) << 4;
unsigned int addr = umb_allocate(size);
is_umb_fn = 1;
Debug0((dbg_fd, "Allocate UMB memory: %#x\n", size));
if (addr == 0) {
int avail=umb_query();
Debug0((dbg_fd, "Allocate UMB Failure\n"));
XMS_RET(avail ? 0xb0 : 0xb1);
LWORD(edx) = avail >> 4;
}
else {
Debug0((dbg_fd, "Allocate UMB Success\n"));
LWORD(eax) = 1;
LWORD(ebx) = addr >> 4;
LWORD(edx) = size >> 4;
}
Debug0((dbg_fd, "umb_allocated: %#x0:%#x0\n",
(unsigned) LWORD(ebx),
(unsigned) LWORD(edx)));
/* retval = UNCHANGED; */
break;
}
/**
* @brief BTRecordFile::readRecordFromDiskTest
* @param pDisk
* @param pRecordID
* Hace la lectura de un registro en la RAM
*/
void BTRecordFile::readRecordFromDiskTest(Disk pDisk, unsigned short pRecordID)
{
const char *padre = pDisk.read(this->_metadataPtr->getFirstRecordPos(), 7);
const char *hizq = pDisk.read(this->_metadataPtr->getFirstRecordPos() + 8, 7);
const char *hder = pDisk.read(this->_metadataPtr->getFirstRecordPos() + 16, 7);
std::string father(padre); // obtiene el padre
std::string HI(hizq); // obtiene el hijo izq
std::string HD(hder); // obtiene el hijo der
unsigned short _sizeCounter = this->_metadataPtr->getFirstRecordPos() + 24; // inicio de la data
DLL<IRecordDataType*> *tmp1 = _metadataPtr->getRecordStruct();
DLLNode<IRecordDataType*> *tmp = tmp1->getHeadPtr();
const char *data;
cout << "Binario " << father << " " << HI << " " << HD << endl;
std::string P = _conversion->binaryToDecimal(father);
std::string PHI = _conversion->binaryToDecimal(HI);
std::string PHD = _conversion->binaryToDecimal(HD);
cout << P << " " << PHI << " " << PHD << " ";
while (tmp != nullptr) {
data = (dynamic_cast<RecordDataType<char>*>(tmp->getData()))->getDataPtr();
const char *DATO = pDisk.read(_sizeCounter, 7);
std::string DATOSTR(DATO); // obtiene el padre
_sizeCounter += 8;
cout << sortUserDataFromDisk(DATOSTR, *data) << endl;
tmp = tmp->getNextPtr();
}
}
//-----------------------------------------------------------------------------
void SetSingleRegister(void)//установить значение одного регистра
{
unsigned char i=0;
unsigned int CRC=0x0;
unsigned int addr;//адрес регистра
unsigned int num;//значение регистра
//-----------установка нужного регистра------------
addr=(((unsigned int)RecieveBuf[2])|((unsigned int)RecieveBuf[3]<<8));
num=(((unsigned int)RecieveBuf[4]<<8)|((unsigned int)RecieveBuf[5]));
if(addr>=REG_ADDR_MIN && addr<=REG_ADDR_MAX)
{
controller_reg[addr]=num;
for (i=0;i<(buf_count-CRC_LEN);i++)
{
TransferBuf[i]=RecieveBuf[i];
}
buf_len=buf_count;
CRC=CRC16(&TransferBuf,buf_len-CRC_LEN);
TransferBuf[buf_len-2]=HI(CRC);
TransferBuf[buf_len-1]=LO(CRC);
}
else
{
//обработка ошибки
MD_STATE=MD_ERR_HANDLING;
}
//-------------------------------------------------
}
void SQuIDS::Derive(double at){
t=at;
PreDerive(at);
for(unsigned int ei = 0; ei < nx; ei++){
// Density matrix
for(unsigned int i = 0; i < nrhos; i++){
// Coherent interaction
if(CoherentRhoTerms)
dstate[ei].rho[i] = iCommutator(estate[ei].rho[i],HI(ei,i,t));
else
dstate[ei].rho[i].SetAllComponents(0.);
// Non coherent interaction
if(NonCoherentRhoTerms)
dstate[ei].rho[i] -= ACommutator(GammaRho(ei,i,t),estate[ei].rho[i]);
// Other possible interaction, for example involving the Scalars or non linear terms in rho.
if(OtherRhoTerms)
dstate[ei].rho[i] += InteractionsRho(ei,i,t);
}
//Scalars
for(unsigned int is=0;is<nscalars;is++){
dstate[ei].scalar[is]=0.;
if(GammaScalarTerms)
dstate[ei].scalar[is] += -estate[ei].scalar[is]*GammaScalar(ei,is,t);
if(OtherScalarTerms)
dstate[ei].scalar[is] += InteractionsScalar(ei,is,t);
}
}
}
static int make_magnitude_modifier(struct iforce* iforce,
struct resource* mod_chunk, int no_alloc, __s16 level)
{
unsigned char data[3];
if (!no_alloc) {
mutex_lock(&iforce->mem_mutex);
if (allocate_resource(&(iforce->device_memory), mod_chunk, 2,
iforce->device_memory.start, iforce->device_memory.end, 2L,
NULL, NULL)) {
mutex_unlock(&iforce->mem_mutex);
return -ENOSPC;
}
mutex_unlock(&iforce->mem_mutex);
}
data[0] = LO(mod_chunk->start);
data[1] = HI(mod_chunk->start);
data[2] = HIFIX80(level);
iforce_send_packet(iforce, FF_CMD_MAGNITUDE, data);
iforce_dump_packet("magnitude: ", FF_CMD_MAGNITUDE, data);
return 0;
}
inline void InitAll(void)
{
//InitUSART
UBRRL = LO(bauddivider);
UBRRH = HI(bauddivider);
UCSRA = 0;
UCSRB = 1<<RXEN|1<<TXEN|0<<RXCIE|0<<TXCIE|0<<UDRIE;
UCSRC = 1<<URSEL|1<<UCSZ0|1<<UCSZ1;
//InitPort
PORTA = 0x00;
DDRA = 0xFF;
//Init ADC
//Init PWM
//Init Interrupts
//Init Timers
}
int dosc_interface(void)
{
switch (HI(ax)) {
case DOSC_NOTIFY: { /* install check and notify */
running_DosC = LWORD(ebx);
LWORD(eax) = (Bit16u)DOSEMU_VERSION_CODE;
LWORD(edx) = DOSEMU_VERSION_CODE >> 16;
c_printf("Booted DosC kernel build %d\n", running_DosC);
break;
}
case DOSC_RUNNING: {
LWORD(eax) = 0;
LWORD(ebx) = running_DosC;
break;
}
case DOSC_GENERIC: {
Bit16u *ssp = SEG_ADR((Bit16u *), ss, sp);
int mode = ssp[1];
switch (mode) {
case E6_INT2f11:
return pass_to_int2f(ssp);
}
}
}
return 1;
}
long
cc3000_spi_write(unsigned char *pUserBuffer, unsigned short usLength)
{
cc3000_irq_disable();
unsigned char ucPad = 0;
// Figure out the total length of the packet in order to figure out if there
// is padding or not
if(!(usLength & 0x0001))
{
ucPad++;
}
pUserBuffer[0] = WRITE;
pUserBuffer[1] = HI(usLength + ucPad);
pUserBuffer[2] = LO(usLength + ucPad);
pUserBuffer[3] = 0;
pUserBuffer[4] = 0;
usLength += (SPI_HEADER_SIZE + ucPad);
// The magic number that resides at the end of the TX/RX buffer (1 byte after
// the allocated size) for the purpose of detection of the overrun. If the
// magic number is overwritten - buffer overrun occurred - and we will stuck
// here forever!
if (wlan_tx_buffer[CC3000_TX_BUFFER_SIZE - 1] != CC3000_BUFFER_MAGIC_NUMBER)
{
while (1)
;
}
if (sSpiInformation.ulSpiState == eSPI_STATE_POWERUP)
{
while (sSpiInformation.ulSpiState != eSPI_STATE_INITIALIZED)
;
}
if (sSpiInformation.ulSpiState == eSPI_STATE_INITIALIZED)
{
// This is time for first TX/RX transactions over SPI: the IRQ is down -
// so need to send read buffer size command
SpiFirstWrite(pUserBuffer, usLength);
}
else
{
// Assert the CS line and wait till SSI IRQ line is active and then
// initialize write operation
ASSERT_CS();
while (cc3000_read_irq_pin()) ; // wait for the IRQ line to go low
SpiWriteDataSynchronous(pUserBuffer, usLength);
DEASSERT_CS();
}
cc3000_irq_enable();
return(0);
}
/*
* Checksum for SRECORD. Add all the bytes from the record length field through the data, and take
* the ones complement. This includes the address field, which for SRECORDs can be 2 bytes, 3 bytes or
* 4 bytes. In this case, since the upper bytes of the address will be 0 for records of the smaller sizes,
* just add all 4 bytes of the address in all cases.
*
* The arguments are:
*
* buf a pointer to the beginning of the data for the record
* length the length of the data portion of the record
* chunk_len the length of the record starting at the address and including the checksum
* chunk_addr starting address for the data in the record
*
* Returns an unsigned char with the checksum
*/
static unsigned char srec_csum(unsigned char *buf, unsigned int length, int chunk_len, int chunk_addr) {
int sum = chunk_len + (LO(chunk_addr)) + (HI(chunk_addr)) + (EX(chunk_addr)) + (EH(chunk_addr));
unsigned int i;
for(i = 0; i < length; i++) {
sum += buf[i];
}
return ~sum & 0xff;
}
int stlink2_swim_write_range(programmer_t *pgm, stm8_device_t *device, unsigned char *buffer, unsigned int start, unsigned int length, const memtype_t memtype) {
stlink2_init_session(pgm);
stlink2_write_byte(pgm, 0x00, device->regs.CLK_CKDIVR);
if(memtype == FLASH || memtype == EEPROM || memtype == OPT) {
stlink2_write_and_read_byte(pgm, 0x00, device->regs.FLASH_IAPSR);
}
if(memtype == FLASH) {
stlink2_write_byte(pgm, 0x56, device->regs.FLASH_PUKR);
stlink2_write_byte(pgm, 0xae, device->regs.FLASH_PUKR);
}
if(memtype == EEPROM || memtype == OPT) {
stlink2_write_byte(pgm, 0xae, device->regs.FLASH_DUKR);
stlink2_write_byte(pgm, 0x56, device->regs.FLASH_DUKR);
}
if(memtype == FLASH || memtype == EEPROM || memtype == OPT) {
stlink2_write_and_read_byte(pgm, 0x56, device->regs.FLASH_IAPSR); // mov 0x56, FLASH_IAPSR
}
int i;
int BLOCK_SIZE = device->flash_block_size;
for(i = 0; i < length; i+=BLOCK_SIZE) {
if(memtype == FLASH || memtype == EEPROM || memtype == OPT) {
// stlink2_write_word(pgm, 0x01fe, device->regs.FLASH_CR2); // mov 0x01fe, FLASH_CR2
stlink2_write_byte(pgm, 0x01, device->regs.FLASH_CR2); // mov 0x01fe, FLASH_CR2; 0x817e - enable write OPT bytes
if(device->regs.FLASH_NCR2 != 0) { // Device have FLASH_NCR2 register
stlink2_write_byte(pgm, 0xFE, device->regs.FLASH_NCR2);
}
}
// The first 8 packet bytes are getting transmitted
// with the same USB bulk transfer as the command itself
msg_init(cmd_buf, 0xf40a);
format_int(&(cmd_buf[2]), BLOCK_SIZE, 2, MP_BIG_ENDIAN);
format_int(&(cmd_buf[6]), start + i, 2, MP_BIG_ENDIAN);
memcpy(&(cmd_buf[8]), &(buffer[i]), 8);
msg_send(pgm, cmd_buf, sizeof(cmd_buf));
// Transmitting the rest
msg_send(pgm, &(buffer[i + 8]), BLOCK_SIZE - 8);
// Waiting for the transfer to process
TRY(128, HI(stlink2_get_status(pgm)) == BLOCK_SIZE);
if(memtype == FLASH || memtype == EEPROM || memtype == OPT) {
stlink2_wait_until_transfer_completes(pgm, device);
}
}
if(memtype == FLASH || memtype == EEPROM || memtype == OPT) {
stlink2_write_and_read_byte(pgm, 0x56, device->regs.FLASH_IAPSR); // mov 0x56, FLASH_IAPSR
}
stlink2_write_byte(pgm, 0x00, 0x7f80);
stlink2_write_byte(pgm, 0xb6, 0x7f80);
stlink2_finish_session(pgm);
return(length);
}
int com_dosgetdrive(void)
{
int ret;
pre_msdos();
HI(ax) = 0x19;
call_msdos(); /* call MSDOS */
ret = LO(ax); /* 0=A, 1=B, ... */
post_msdos();
return ret;
}
// Backpatch the location loc with the value of offset for branch instructions
int backpatch(u2 loc, int offset)
{
if (loc + offset < 0 || loc + offset >= 65534)
return 1;
codebuf[loc + 1] = HI(offset);
codebuf[loc + 2] = LO(offset);
return 0;
}
void start ()
/* Takes no parameters.
* Unknown calling convention.
* Contains instructions not normally used by compilers.
*/
{
int loc1; /* di */
int loc2; /* si */
int loc3; /* ch */
int loc4; /* cx */
long loc5; /* dx:ax */
int loc6; /* bl */
int loc7; /* al */
int loc8; /* bx */
int loc9; /* dh */
loc1 = 0;
loc2 = 395;
do {
*loc1 = loc3;
loc4 = (loc4 + loc2);
loc5 = (40 * loc4);
loc2 = (loc2 - HI(loc5));
} while ((++loc1 != 0));
HI(loc5) = LO(loc5);
for (;;) {
loc2 = 0;
loc6 = 200;
do {
loc4 = 320;
do {
loc1 = loc4;
loc7 = (*loc8 + *loc1);
es[si] = ((loc7 + *((loc1 + HI(loc5)))) & loc9);
loc2 = (loc2 + 1);
} while (//*failed*//);
} while ((--loc6 != 0));
HI(loc5) = (HI(loc5) + 1);
} /* end of loop */
}
//-----------------------------------------------------------------------------
void ReadHoldingReg(void)//чтение регистров хранения
{
#define HEAD_LEN 3
//
//[addr|func|n_regs|....|crc|crc]-кадр ответа
//[ head |............]
unsigned char i=0,count=0;
unsigned int CRC=0x0;
unsigned int addr;//адрес начального регистра
unsigned int len;//количество считываемых регистров
TransferBuf[0]=ADRESS_DEV;
TransferBuf[1]=READ_HOLDING_REG;//прочитать регистры хранения
//-----------чтение начального адреса и количества регистров------------
addr=(((unsigned int)RecieveBuf[2]<<8)|(unsigned int)RecieveBuf[3]);
len= (((unsigned int)RecieveBuf[4]<<8)|(unsigned int)RecieveBuf[5]);
if(addr>=REG_ADDR_MIN && addr<=REG_ADDR_MAX && (len+addr)<=REG_ADDR_MAX)
{
for(i=addr;i<(addr+len);i++)
{
TransferBuf[count+HEAD_LEN]=HI(controller_reg[i]);
TransferBuf[count+HEAD_LEN+1]=LO(controller_reg[i]);
count+=2;
}
TransferBuf[2]=count; //счетчик байт
buf_len=HEAD_LEN+count+CRC_LEN; //длина передаваемого буфера
CRC=CRC16(&TransferBuf,buf_len-CRC_LEN);
TransferBuf[buf_len-2]=HI(CRC);
TransferBuf[buf_len-1]=LO(CRC);
}
else
{
//обработка ошибки
MD_STATE=MD_ERR_HANDLING;
}
//-------------------------------------------------
}
int stlink2_write_byte(programmer_t *pgm, unsigned char byte, unsigned int start) {
unsigned char buf[4], start2[2];
pack_int16(start, start2);
stlink2_cmd(pgm, 0xf40a, 7,
0x00, 0x01,
0x00, 0x00,
HI(start), LO(start),
byte);
usleep(2000);
return(stlink2_get_status(pgm)); // Should be '1'
}
void i2c_sdaOut(uint8_t value){
if (value) {
PINMODE(SDA_DDR, SDA,INPUT);
led_g_on();
HI(SDA_PORT,SDA);
} else {
PINMODE(SDA_DDR, SDA,OUTPUT);
led_g_off();
LO(SDA_PORT,SDA);
}
}
//-----------------------------------------------------------------------------
void Send_Info(void)//информация об устройстве
{
unsigned int CRC=0x0;
TransferBuf[0]=ADRESS_DEV;
TransferBuf[1]=0x11;//получить информацию
TransferBuf[2]=0x4; //счетчик байт
TransferBuf[3]=HI(DEV_ID);//id
TransferBuf[4]=LO(DEV_ID);//id
TransferBuf[5]=HI(DEV_SN);//SN
TransferBuf[6]=LO(DEV_SN);//SN
CRC=CRC16(&TransferBuf,7);
TransferBuf[7]=HI(CRC);
TransferBuf[8]=LO(CRC);
buf_len=0x9;
}
