static int make_condition_modifier(struct iforce* iforce,
struct resource* mod_chunk, int no_alloc,
__u16 rsat, __u16 lsat, __s16 rk, __s16 lk, u16 db, __s16 center)
{
unsigned char data[10];
if (!no_alloc) {
mutex_lock(&iforce->mem_mutex);
if (allocate_resource(&(iforce->device_memory), mod_chunk, 8,
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] = (100 * rk) >> 15;
data[3] = (100 * lk) >> 15;
center = (500 * center) >> 15;
data[4] = LO(center);
data[5] = HI(center);
db = (1000 * db) >> 16;
data[6] = LO(db);
data[7] = HI(db);
data[8] = (100 * rsat) >> 16;
data[9] = (100 * lsat) >> 16;
iforce_send_packet(iforce, FF_CMD_CONDITION, data);
iforce_dump_packet("condition", FF_CMD_CONDITION, data);
return 0;
}
static int make_condition_modifier(struct iforce* iforce,
struct resource* mod_chunk, int no_alloc,
__u16 rsat, __u16 lsat, __s16 rk, __s16 lk, u16 db, __s16 center)
{
unsigned char data[10];
if (!no_alloc) {
mutex_lock(&iforce->mem_mutex);
if (allocate_resource(&(iforce->device_memory), mod_chunk, 8,
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] = (100 * rk) >> 15; /* Dangerous: the sign is extended by gcc on plateforms providing an arith shift */
data[3] = (100 * lk) >> 15; /* This code is incorrect on cpus lacking arith shift */
center = (500 * center) >> 15;
data[4] = LO(center);
data[5] = HI(center);
db = (1000 * db) >> 16;
data[6] = LO(db);
data[7] = HI(db);
data[8] = (100 * rsat) >> 16;
data[9] = (100 * lsat) >> 16;
iforce_send_packet(iforce, FF_CMD_CONDITION, data);
iforce_dump_packet("condition", FF_CMD_CONDITION, data);
return 0;
}
void usart_init(void)
{
UBRR0L = LO(BAUDDIVIDER);
UBRR0H = HI(BAUDDIVIDER);
UCSR0B = 1<<RXEN0|1<<TXEN0|1<<RXCIE0|1<<TXCIE0;
UCSR0C = 1<<UCSZ00|1<<UCSZ01;
usart_rx_wp = 0;
usart_rx_rp = 0;
usart_tx_wp = 0;
usart_tx_rp = 0;
}
void ads_read(uint8_t chip, sample_data* sample){
ads_select(chip);
uint8_t* buf=(uint8_t*)sample;
uint8_t* end=(uint8_t*)(sample+(sizeof(sample_data)));
while(buf != end){
*(buf++)=spi_send(0x00);
}
for(uint8_t i=0; i<4; i++){
uint16_t tmp = ((LO(sample->ch[i])<<8) | (HI(sample->ch[i])));
sample->ch[i] = tmp;
}
ads_deselect();
}
int stlink2_write_and_read_byte(programmer_t *pgm, unsigned char byte, unsigned int start) {
unsigned char buf[4], start2[2];
pack_int16(start, start2);
stlink2_cmd(pgm, 0xf40b, 7,
0x00, 0x01,
0x00, 0x00,
HI(start), LO(start),
byte);
usleep(2000);
stlink2_get_status(pgm);
stlink2_cmd(pgm, 0xf40c, 0);
return(msg_recv_int8(pgm));
}
static int make_envelope_modifier(struct iforce* iforce,
struct resource* mod_chunk, int no_alloc,
u16 attack_duration, __s16 initial_level,
u16 fade_duration, __s16 final_level)
{
unsigned char data[8];
attack_duration = TIME_SCALE(attack_duration);
fade_duration = TIME_SCALE(fade_duration);
if (!no_alloc) {
mutex_lock(&iforce->mem_mutex);
if (allocate_resource(&(iforce->device_memory), mod_chunk, 0x0e,
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] = LO(attack_duration);
data[3] = HI(attack_duration);
data[4] = HI(initial_level);
data[5] = LO(fade_duration);
data[6] = HI(fade_duration);
data[7] = HI(final_level);
iforce_send_packet(iforce, FF_CMD_ENVELOPE, data);
return 0;
}
//RTOS Запуск системного таймера
inline void RunRTOS (void)
{
// TCCR2A = 1 << WGM21 | 4 << CS20; // Freq = CK/64 - Установить режим и предделитель
// Автосброс после достижения регистра сравнения
TCCR2B |= (1<<CS22);
TCCR2B &= ~((1<<CS21) | (1<<CS20));
TCCR2B |= (1<<WGM22);
TCNT2 = 0; // Установить начальное значение счётчиков
OCR2B = LO(TimerDivider); // Установить значение в регистр сравнения
//TIMSK2 = 0 << TOIE0 | 1<<OCF2A | 0<<TOIE0; // Разрешаем прерывание RTOS - запуск ОС
TIMSK2 = 1 << TOIE2;
sei();
}
inline void InitAll(void)
{
//InitUSART
UBRRL = LO(bauddivider);
UBRRH = HI(bauddivider);
UCSRA = 0;
UCSRB = 1<<RXEN|1<<TXEN|0<<RXCIE|0<<TXCIE;
UCSRC = 1<<URSEL|1<<UCSZ0|1<<UCSZ1;
//InitPort
LED_DDR |= 1<<LED1|1<<LED2|1<<LED3|1<<I_L|1<<I_C;
}
long SpiSetUp(unsigned char *pUserBuffer, unsigned short usLength)
{
size_t tx_len = (usLength & 1) ? usLength : usLength +1;
pUserBuffer[0] = WRITE;
pUserBuffer[1] = HI(tx_len);
pUserBuffer[2] = LO(tx_len);
pUserBuffer[3] = 0;
pUserBuffer[4] = 0;
tSLInformation.solicitedResponse = 1; // We are doing a write
sSpiInformation.pTxPacket = pUserBuffer;
sSpiInformation.usTxPacketLength = usLength;
tx_len += SPI_HEADER_SIZE;
return tx_len;
}
//---------------------------------------------------------------------------------------------
void stub(void)//заглушка для проверки
{
unsigned int CRC=0;
TransferBuf[0]=ADRESS_DEV;
TransferBuf[1]=0x3;//read reg
TransferBuf[2]=0x2;
TransferBuf[3]=0x1;
TransferBuf[4]=0x1;
//TransferBuf[5]=0x1;
CRC=CRC16(&TransferBuf,5);
TransferBuf[5]=HI(CRC);
TransferBuf[6]=LO(CRC);
buf_len=0x7;
}
/**
Инициализировать UART
*/
void UART::init(void) {
// задаем скорость UART
UBRRH = HI(BAUDRATE_DIVIDER);
UBRRL = LO(BAUDRATE_DIVIDER);
UCSRA = 0;
// включаем трансмиттер и ресивер
UCSRB = 1 << TXEN | 1 << RXEN | 1 << RXCIE;
// контроль четности: нет
// стоп бит: 1
// размер кадра 8 бит
UCSRC = (1 << URSEL) | (1 << UCSZ0) | (1 << UCSZ1);
receiver.init();
}
static void remote_ahci_get_num_blocks(ddf_fun_t *fun, void *iface,
ipc_callid_t callid, ipc_call_t *call)
{
const ahci_iface_t *ahci_iface = (ahci_iface_t *) iface;
if (ahci_iface->get_num_blocks == NULL) {
async_answer_0(callid, ENOTSUP);
return;
}
uint64_t blocks;
const int ret = ahci_iface->get_num_blocks(fun, &blocks);
if (ret != EOK)
async_answer_0(callid, ret);
else
async_answer_2(callid, EOK, HI(blocks), LO(blocks));
}
int ahci_write_blocks(async_sess_t *sess, uint64_t blocknum, size_t count,
void* buf)
{
async_exch_t *exch = async_exchange_begin(sess);
if (!exch)
return EINVAL;
aid_t req = async_send_4(exch, DEV_IFACE_ID(AHCI_DEV_IFACE),
IPC_M_AHCI_WRITE_BLOCKS, HI(blocknum), LO(blocknum), count, NULL);
async_share_out_start(exch, buf, AS_AREA_READ | AS_AREA_WRITE);
async_exchange_end(exch);
sysarg_t rc;
async_wait_for(req, &rc);
return rc;
}
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 */
}
inline std::string ToString(const Direction& d) {
std::stringstream sst;
std::stringstream sst2;
unsigned short hi = HI(d);
unsigned short lo = LO(d);
if ((hi & 1) == 1) {
sst << "X";
sst2 << (((lo & 1) == 1) ? "P" : "M");
}
if ((hi & 2) == 2) {
sst << "Y";
sst2 << (((lo & 2) == 2) ? "P" : "M");
}
if ((hi & 4) == 4) {
sst << "Z";
sst2 << (((lo & 4) == 4) ? "P" : "M");
}
sst << "_" << sst2.str();
return sst.str();
}
void proc_1 (int arg0)
/* Uses register arguments:
* arg0 = ax.
* Unknown calling convention.
* Contains instructions not normally used by compilers.
*/
{
int loc1; /* bx */
int loc2; /* dx */
long loc3; /* dx:ax */
int loc4; /* al */
loc1 = arg0;
arg0 = 0x34dd;
loc2 = 18;
if (loc2 < loc1) {
arg0 = (loc3 / loc1);
loc1 = LO(loc3);
if ((loc4 & 3) == 0) {
}
}
}
void cdrom_helper(unsigned char *req_buf, unsigned char *transfer_buf,
unsigned int dos_transfer_buf)
{
unsigned int Sector_plus_150,Sector;
struct cdrom_msf cdrom_msf;
struct cdrom_subchnl cdrom_subchnl;
struct cdrom_tochdr cdrom_tochdr;
struct cdrom_tocentry cdrom_tocentry;
struct cdrom_volctrl cdrom_volctrl;
int n, err;
cdrom_subchnl.cdsc_format = CDROM_MSF;
IndexCd=(int)((HI(ax) & 0xC0)>>6);
HI(ax) = HI(ax) & 0x3F;
if ((cdu33a) && (cdrom_fd < 0)) {
cdrom_fd = open (path_cdrom, O_RDONLY | O_NONBLOCK);
if (cdrom_fd < 0) {
switch (HI(ax)) {
case 0x09: /* media changed request */
LO(bx) = 1; /* media changed */
LO(ax) = 0;
return;
case 0x0A: /* device status request */
LWORD(ebx) = audio_status.status | 0x800; /* no disc */
LO(ax) = 0;
return;
}
LO(ax) = 1; /* for other requests return with error */
return ;
}
}
switch (HI(ax)) {
case 0x01: /* NOTE: you can't see XA data disks if bit 10 of status
* is cleared, MSCDEX will test it and skip XA entries!
* Actually the entries skipped must have this pattern:
* xxxx1xxx xxxxxxxx 0x58 0x41
* and the mscdex 2.25 code is:
* test word ptr [bx+1Eh],400h
* jz [check for XA]
* [return 0 = valid entry]
* [check for XA]
* ...
* cmp word ptr es:[bx+6],4158h 'XA'
* jne [return 0]
* mov ax,es:[bx+4]
* and ax,8
* [return ax]
*/
audio_status.status = 0x00000710; /* see function 0x0A below */
audio_status.paused_bit = 0;
audio_status.media_changed = 0;
audio_status.volume0 = 0xFF;
audio_status.volume1 = 0xFF;
audio_status.volume2 = 0;
audio_status.volume3 = 0;
audio_status.outchan0 = 0;
audio_status.outchan1 = 1;
audio_status.outchan2 = 2;
audio_status.outchan3 = 3;
cdrom_fd = open (path_cdrom, O_RDONLY | O_NONBLOCK);
err = errno;
if (cdrom_fd < 0) {
C_printf("CDROM: cdrom open (%s) failed: %s\n",
path_cdrom, strerror(err));
LO(ax) = 0;
if ((err == EIO) || (err==ENOMEDIUM)) {
/* drive which cannot be opened if no
disc is inserted! */
cdu33a = 1;
if (! eject_allowed)
LO(ax) = 1; /* no disk in drive */
}
else LO(ax) = 1; /* no cdrom drive installed */
if (! eject_allowed)
LO(ax) = 1; /* no disk in drive */
}
else {
LO(ax) = 0;
if (! eject_allowed) {
if (ioctl (cdrom_fd, CDROMREADTOCHDR, &cdrom_tochdr))
if (ioctl (cdrom_fd, CDROMREADTOCHDR, &cdrom_tochdr))
LO(ax) = 1;
}
}
break;
case 0x02: /* read long */
if (eject_allowed && ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl) && errno != ENOTTY) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
/* no disc in drive */
LO(ax) = 1;
break;
}
else { /* disc in drive */
}
}
if (req_buf == NULL && transfer_buf == NULL) {
req_buf = SEG_ADR((unsigned char *), es, di);
dos_transfer_buf = SEGOFF2LINEAR(REG(ds), LWORD(esi));
}
if (*CALC_PTR(req_buf,MSCD_READ_ADRESSING,u_char) == 1) {
cdrom_msf.cdmsf_min0 = *CALC_PTR(req_buf,MSCD_READ_STARTSECTOR+2,u_char);
cdrom_msf.cdmsf_sec0 = *CALC_PTR(req_buf,MSCD_READ_STARTSECTOR+1,u_char);
cdrom_msf.cdmsf_frame0 = *CALC_PTR(req_buf,MSCD_READ_STARTSECTOR+0,u_char);
Sector = cdrom_msf.cdmsf_min0*60*75+cdrom_msf.cdmsf_sec0*75
+cdrom_msf.cdmsf_frame0-150;
}
else { Sector = *CALC_PTR(req_buf,MSCD_READ_STARTSECTOR,u_long);
}
C_printf("CDROM: reading sector %#x (fmt %d)\n", Sector,
*CALC_PTR(req_buf,MSCD_READ_ADRESSING,u_char));
if ((off_t) -1 == lseek (cdrom_fd, Sector*CD_FRAMESIZE, SEEK_SET)) {
HI(ax) = (errno == EINVAL ? 0x08 : 0x0F);
C_printf("CDROM: lseek failed: %s\n", strerror(errno));
LO(ax) = 1;
} else {
n = *CALC_PTR(req_buf,MSCD_READ_NUMSECTORS,u_short)*CD_FRAMESIZE;
if (transfer_buf == NULL) {
n = dos_read (cdrom_fd, dos_transfer_buf, n);
} else {
n = unix_read (cdrom_fd, transfer_buf, n);
}
if ( n < 0 ) {
/* cd must be in drive, reset drive and try again */
cdrom_reset();
if ((off_t) -1 == lseek (cdrom_fd, Sector*CD_FRAMESIZE, SEEK_SET)) {
HI(ax) = (errno == EINVAL ? 0x08 : 0x0F);
C_printf("CDROM: lseek failed: %s\n", strerror(errno));
LO(ax) = 1;
} else {
n = *CALC_PTR(req_buf,MSCD_READ_NUMSECTORS,u_short)*CD_FRAMESIZE;
if (transfer_buf == NULL)
n = dos_read (cdrom_fd, dos_transfer_buf, n);
else
n = unix_read (cdrom_fd, transfer_buf, n);
if ( n < 0) {
HI(ax) = (errno == EFAULT ? 0x0A : 0x0F);
C_printf("CDROM: sector read (to %p, len %#x) failed: %s\n",
transfer_buf, *CALC_PTR(req_buf,MSCD_READ_NUMSECTORS,u_short)*CD_FRAMESIZE, strerror(errno));
LO(ax) = 1;
} else LO(ax) = 0;
}
}
if (n != *CALC_PTR(req_buf,MSCD_READ_NUMSECTORS,u_short)*CD_FRAMESIZE) {
C_printf("CDROM: sector read len %#x got %#x\n",
*CALC_PTR(req_buf,MSCD_READ_NUMSECTORS,u_short)*CD_FRAMESIZE, n);
LO(ax) = 1;
HI(ax) = 0x0F;
} else {
#ifdef CDROM_DEBUG
dump_cd_sect(transfer_buf);
#endif
LO(ax) = 0;
}
}
break;
case 0x03: /* seek */
req_buf = SEG_ADR((unsigned char *), es, di);
if ((off_t)-1 == lseek (cdrom_fd, *CALC_PTR(req_buf,MSCD_SEEK_STARTSECTOR,u_long)*CD_FRAMESIZE, SEEK_SET)) {
C_printf("CDROM: lseek failed: %s\n", strerror(errno));
LO(ax) = 1;
}
break;
case 0x04: /* play */
req_buf = SEG_ADR((unsigned char *), es, di);
if (*CALC_PTR(req_buf,MSCD_PLAY_ADRESSING,u_char) == 1) {
cdrom_msf.cdmsf_min0 = *CALC_PTR(req_buf,MSCD_PLAY_STARTSECTOR+2,u_char);
cdrom_msf.cdmsf_sec0 = *CALC_PTR(req_buf,MSCD_PLAY_STARTSECTOR+1,u_char);
cdrom_msf.cdmsf_frame0 = *CALC_PTR(req_buf,MSCD_PLAY_STARTSECTOR+0,u_char);
Sector_plus_150 = cdrom_msf.cdmsf_min0*60*75+cdrom_msf.cdmsf_sec0*75
+cdrom_msf.cdmsf_frame0;
audio_status.last_StartSector = Sector_plus_150;
}
else { Sector_plus_150 = *CALC_PTR(req_buf,MSCD_PLAY_STARTSECTOR,u_long) + 150;
cdrom_msf.cdmsf_min0 = (Sector_plus_150 / (60*75));
cdrom_msf.cdmsf_sec0 = (Sector_plus_150 % (60*75)) / 75;
cdrom_msf.cdmsf_frame0 = (Sector_plus_150 % (60*75)) % 75;
audio_status.last_StartSector = Sector_plus_150;
}
Sector_plus_150 += *CALC_PTR(req_buf,MSCD_PLAY_NUMSECTORS,u_long);
cdrom_msf.cdmsf_min1 = (Sector_plus_150 / (60*75));
cdrom_msf.cdmsf_sec1 = (Sector_plus_150 % (60*75)) / 75;
cdrom_msf.cdmsf_frame1 = (Sector_plus_150 % (60*75)) % 75;
audio_status.last_EndSector = Sector_plus_150;
audio_status.paused_bit = 0;
if (ioctl (cdrom_fd, CDROMPLAYMSF, &cdrom_msf)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMPLAYMSF, &cdrom_msf)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
LO(ax) = 0;
break;
case 0x05: /* pause (stop) audio */
LO(ax) = 0;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl) == 0) {
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY) {
audio_status.last_StartSector =
cdrom_subchnl.cdsc_absaddr.msf.minute*60*75
+cdrom_subchnl.cdsc_absaddr.msf.second*75
+cdrom_subchnl.cdsc_absaddr.msf.frame;
ioctl (cdrom_fd, CDROMPAUSE, NULL);
audio_status.paused_bit = 1;
}
else { audio_status.last_StartSector = 0;
audio_status.last_EndSector = 0;
audio_status.paused_bit = 0;
}
}
else { audio_status.last_StartSector = 0;
audio_status.last_EndSector = 0;
audio_status.paused_bit = 0;
audio_status.media_changed = 1;
}
break;
case 0x06: /* resume audio */
LO(ax) = 0;
if (audio_status.paused_bit) {
if (ioctl (cdrom_fd, CDROMRESUME, NULL) == 0) {
audio_status.paused_bit = 0;
HI(ax) = 1;
}
}
else LO(ax) = 1;
break;
case 0x07: /* location of head */
LWORD(eax) = 0;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY)
HI(ax) = 1;
req_buf = SEG_ADR((unsigned char *), ds, si);
if (*CALC_PTR(req_buf,MSCD_LOCH_ADRESSING,u_char) == 0) {
*CALC_PTR(req_buf,MSCD_LOCH_LOCATION,u_long)
= cdrom_subchnl.cdsc_absaddr.msf.minute*60*75
+cdrom_subchnl.cdsc_absaddr.msf.second*75
+cdrom_subchnl.cdsc_absaddr.msf.frame-150;
}
else {/* red book adressing */
*CALC_PTR(req_buf,MSCD_LOCH_LOCATION+3,u_char) = 0;
*CALC_PTR(req_buf,MSCD_LOCH_LOCATION+2,u_char) = cdrom_subchnl.cdsc_absaddr.msf.minute;
*CALC_PTR(req_buf,MSCD_LOCH_LOCATION+1,u_char) = cdrom_subchnl.cdsc_absaddr.msf.second;
*CALC_PTR(req_buf,MSCD_LOCH_LOCATION+0,u_char) = cdrom_subchnl.cdsc_absaddr.msf.frame;
}
break;
case 0x08: /* return sectorsize */
LO(ax) = 0;
LWORD(ebx) = CD_FRAMESIZE;
break;
case 0x09: /* media changed */
/* this function will be called from MSCDEX before
each new disk access ! */
HI(ax) = 0; LO(ax) = 0; LO(bx) = 0;
C_printf("CDROM: media changed? %#x\n", audio_status.media_changed);
errno = 0;
if (eject_allowed) {
if ((audio_status.media_changed) ||
ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
if (errno == EIO)
cdrom_reset();
audio_status.media_changed = 0;
LO(bx) = 1; /* media has been changed */
C_printf("CDROM: media changed? yes\n");
ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl);
if (! ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl))
cdrom_reset(); /* disc in drive */
}
else /* media has not changed, check audio status */
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY)
HI(ax) = 1; /* audio playing in progress */
}
break;
case 0x0A: /* device status */
HI(ax) = 0; LO(ax) = 0;
if (eject_allowed) {
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl))
{ /* no disk in drive */
LWORD(ebx) = audio_status.status | 0x800;
C_printf("CDROM: subch failed: %s\n", strerror(errno));
break;
}
else cdrom_reset();
}
}
/* disk in drive */
LWORD(ebx) = audio_status.status;
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY)
HI(ax) = 1;
break;
case 0x0B: /* drive reset */
LO(ax) = 0;
break;
case 0x0C: /* lock/unlock door */
cdrom_reset();
if (LO(bx) == 1)
audio_status.status &= 0xFFFFFFFD;
else audio_status.status |= 0x2;
LO(ax) = 0;
break;
case 0x0D: /* eject */
LO(ax) = 0;
if ((eject_allowed) && (audio_status.status & 0x02)) /* drive unlocked ? */
{
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMEJECT)) {
LO(ax) = errno;
}
}
break;
case 0x0E: /* close tray */
LO(ax) = 0;
if ((eject_allowed) && (audio_status.status & 0x02)) /* drive unlocked ? */
{
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMCLOSETRAY)) {
LO(ax) = errno;
}
}
break;
case 0x0F: /* audio channel control */
LWORD(eax) = 0;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY)
HI(ax) = 1;
req_buf = SEG_ADR((unsigned char *), ds, si);
cdrom_volctrl.channel0 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME0, u_char);
cdrom_volctrl.channel1 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME1, u_char);
cdrom_volctrl.channel2 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME2, u_char);
cdrom_volctrl.channel3 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME3, u_char);
audio_status.volume0 = cdrom_volctrl.channel0;
audio_status.volume1 = cdrom_volctrl.channel1;
audio_status.volume2 = cdrom_volctrl.channel2;
audio_status.volume3 = cdrom_volctrl.channel3;
audio_status.outchan0 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME0-1, u_char);
audio_status.outchan1 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME1-1, u_char);
audio_status.outchan2 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME2-1, u_char);
audio_status.outchan3 = *CALC_PTR(req_buf, MSCD_CTRL_VOLUME3-1, u_char);
ioctl (cdrom_fd, CDROMVOLCTRL, &cdrom_volctrl);
break;
case 0x10: /* audio disk info */
LWORD(eax) = 0;
if (ioctl (cdrom_fd, CDROMREADTOCHDR, &cdrom_tochdr)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMREADTOCHDR, &cdrom_tochdr)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
req_buf = SEG_ADR((unsigned char *), ds, si);
*CALC_PTR(req_buf,MSCD_DISKINFO_LTN,u_char) = cdrom_tochdr.cdth_trk0;
*CALC_PTR(req_buf,MSCD_DISKINFO_HTN,u_char) = cdrom_tochdr.cdth_trk1;
cdrom_tocentry.cdte_track = CDROM_LEADOUT;
cdrom_tocentry.cdte_format = CDROM_MSF;
if (ioctl (cdrom_fd, CDROMREADTOCENTRY, &cdrom_tocentry)) {
C_printf ("Fatal cdrom error(audio disk info); read toc header succeeded but following read entry didn't\n");
LO(ax) = 1;
break;
}
#ifdef __linux__
*CALC_PTR(req_buf,MSCD_DISKINFO_LEADOUT+3,u_char) = 0;
*CALC_PTR(req_buf,MSCD_DISKINFO_LEADOUT+2,u_char) = cdrom_tocentry.cdte_addr.msf.minute;
*CALC_PTR(req_buf,MSCD_DISKINFO_LEADOUT+1,u_char) = cdrom_tocentry.cdte_addr.msf.second;
*CALC_PTR(req_buf,MSCD_DISKINFO_LEADOUT+0,u_char) = cdrom_tocentry.cdte_addr.msf.frame;
#endif
break;
case 0x11: /* track info */
req_buf = SEG_ADR((unsigned char *), ds, si);
cdrom_tocentry.cdte_track = *CALC_PTR(req_buf,MSCD_TRACKINFO_TRACKNUM,u_char);
cdrom_tocentry.cdte_format = CDROM_MSF;
C_printf("CDROM: track info, track %d\n", cdrom_tocentry.cdte_track);
if (ioctl (cdrom_fd, CDROMREADTOCENTRY, &cdrom_tocentry)) {
/* XXX MSCDEX reads beyond the end of existing tracks. Sigh. */
if (errno != EINVAL)
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMREADTOCENTRY, &cdrom_tocentry)) {
if (errno == EIO) {
audio_status.media_changed = 1;
/* no disk in drive */
}
LO(ax) = 1;
break;
}
}
#ifdef __linux__
*CALC_PTR(req_buf,MSCD_TRACKINFO_TRACKPOS+3,u_char) = 0;
*CALC_PTR(req_buf,MSCD_TRACKINFO_TRACKPOS+2,u_char) = cdrom_tocentry.cdte_addr.msf.minute;
*CALC_PTR(req_buf,MSCD_TRACKINFO_TRACKPOS+1,u_char) = cdrom_tocentry.cdte_addr.msf.second;
*CALC_PTR(req_buf,MSCD_TRACKINFO_TRACKPOS+0,u_char) = cdrom_tocentry.cdte_addr.msf.frame;
#endif
*CALC_PTR(req_buf,MSCD_TRACKINFO_CTRL,u_char) = cdrom_tocentry.cdte_ctrl << 4 | 0x20;
LO(ax) = 0;
break;
case 0x12: /* volume size */
cdrom_tocentry.cdte_track = CDROM_LEADOUT;
cdrom_tocentry.cdte_format = CDROM_MSF;
if (ioctl (cdrom_fd, CDROMREADTOCENTRY, &cdrom_tocentry)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMREADTOCENTRY, &cdrom_tocentry)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
req_buf = SEG_ADR((unsigned char *), ds, si);
#ifdef __linux__
*CALC_PTR(req_buf,MSCD_GETVOLUMESIZE_SIZE,int) = cdrom_tocentry.cdte_addr.msf.minute*60*75
+cdrom_tocentry.cdte_addr.msf.second*60
+cdrom_tocentry.cdte_addr.msf.frame;
#endif
LO(ax) = 0;
break;
case 0x13: /* q channel */
LWORD(eax) = 0;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY)
HI(ax) = 1;
req_buf = SEG_ADR((unsigned char *), ds, si);
*CALC_PTR(req_buf,MSCD_QCHAN_CTRL,u_char) = (cdrom_subchnl.cdsc_adr << 4) + (cdrom_subchnl.cdsc_ctrl);
*CALC_PTR(req_buf,MSCD_QCHAN_TNO,u_char) = cdrom_subchnl.cdsc_trk;
*CALC_PTR(req_buf,MSCD_QCHAN_IND,u_char) = cdrom_subchnl.cdsc_ind;
#ifdef __linux__
*CALC_PTR(req_buf,MSCD_QCHAN_MIN,u_char) = cdrom_subchnl.cdsc_reladdr.msf.minute;
*CALC_PTR(req_buf,MSCD_QCHAN_SEC,u_char) = cdrom_subchnl.cdsc_reladdr.msf.second;
*CALC_PTR(req_buf,MSCD_QCHAN_FRM,u_char) = cdrom_subchnl.cdsc_reladdr.msf.frame;
#endif
*CALC_PTR(req_buf,MSCD_QCHAN_ZERO,u_char) = 0;
*CALC_PTR(req_buf,MSCD_QCHAN_AMIN,u_char) = cdrom_subchnl.cdsc_absaddr.msf.minute;
*CALC_PTR(req_buf,MSCD_QCHAN_ASEC,u_char) = cdrom_subchnl.cdsc_absaddr.msf.second;
*CALC_PTR(req_buf,MSCD_QCHAN_AFRM,u_char) = cdrom_subchnl.cdsc_absaddr.msf.frame;
break;
case 0x14: /* audio status */
LWORD(eax) = 0;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY)
HI(ax) = 1;
req_buf = SEG_ADR((unsigned char *), ds, si);
*CALC_PTR(req_buf,MSCD_AUDSTAT_PAUSED,u_short)= audio_status.paused_bit;
*CALC_PTR(req_buf,MSCD_AUDSTAT_START ,u_long) = audio_status.last_StartSector;
*CALC_PTR(req_buf,MSCD_AUDSTAT_END ,u_long) = audio_status.last_EndSector;
break;
case 0x15: /* get audio channel information */
LWORD(eax) = 0;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
audio_status.media_changed = 1;
if (ioctl (cdrom_fd, CDROMSUBCHNL, &cdrom_subchnl)) {
/* no disk in drive */
LO(ax) = 1;
break;
}
}
if (cdrom_subchnl.cdsc_audiostatus == CDROM_AUDIO_PLAY)
HI(ax) = 1;
req_buf = SEG_ADR((unsigned char *), ds, si);
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME0,u_char) = audio_status.volume0;
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME1,u_char) = audio_status.volume1;
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME2,u_char) = audio_status.volume2;
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME3,u_char) = audio_status.volume3;
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME0-1,u_char) = audio_status.outchan0;
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME1-1,u_char) = audio_status.outchan1;
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME2-1,u_char) = audio_status.outchan2;
*CALC_PTR(req_buf,MSCD_AUDCHAN_VOLUME3-1,u_char) = audio_status.outchan3;
break;
default: C_printf ("CDROM: unknown request %#x!\n",HI(ax));
}
//*****************************************************************************
//
//! SpiWrite
//!
//! @param pUserBuffer buffer to write
//! @param usLength buffer's length
//!
//! @return none
//!
//! @brief Spi write operation
//
//*****************************************************************************
long
SpiWrite(unsigned char *pUserBuffer, unsigned short usLength)
{
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
{
// We need to prevent here race that can occur in case 2 back to back
// packets are sent to the device, so the state will move to IDLE and once
//again to not IDLE due to IRQ
tSLInformation.WlanInterruptDisable();
while (sSpiInformation.ulSpiState != eSPI_STATE_IDLE)
{
;
}
sSpiInformation.ulSpiState = eSPI_STATE_WRITE_IRQ;
sSpiInformation.pTxPacket = pUserBuffer;
sSpiInformation.usTxPacketLength = usLength;
// Assert the CS line and wait till SSI IRQ line is active and then
// initialize write operation
ASSERT_CS();
// Re-enable IRQ - if it was not disabled - this is not a problem...
tSLInformation.WlanInterruptEnable();
// check for a missing interrupt between the CS assertion and enabling back the interrupts
if (tSLInformation.ReadWlanInterruptPin() == 0)
{
SpiWriteDataSynchronous(sSpiInformation.pTxPacket, sSpiInformation.usTxPacketLength);
sSpiInformation.ulSpiState = eSPI_STATE_IDLE;
DEASSERT_CS();
}
}
// Due to the fact that we are currently implementing a blocking situation
// here we will wait till end of transaction
while (eSPI_STATE_IDLE != sSpiInformation.ulSpiState)
;
return(0);
}
void led_g_off() { LO(LEDG_PORT,LEDG); }
void i2c_sclLo() {
i2c_hold();
led_y_off();
LO(SCL_PORT,SCL);
}
/*
* Send a packet of bytes to the device
*/
int iforce_send_packet(struct iforce *iforce, u16 cmd, unsigned char* data)
{
/* Copy data to buffer */
int n = LO(cmd);
int c;
int empty;
int head, tail;
unsigned long flags;
/*
* Update head and tail of xmit buffer
*/
spin_lock_irqsave(&iforce->xmit_lock, flags);
head = iforce->xmit.head;
tail = iforce->xmit.tail;
if (CIRC_SPACE(head, tail, XMIT_SIZE) < n+2) {
warn("not enough space in xmit buffer to send new packet");
spin_unlock_irqrestore(&iforce->xmit_lock, flags);
return -1;
}
empty = head == tail;
XMIT_INC(iforce->xmit.head, n+2);
/*
* Store packet in xmit buffer
*/
iforce->xmit.buf[head] = HI(cmd);
XMIT_INC(head, 1);
iforce->xmit.buf[head] = LO(cmd);
XMIT_INC(head, 1);
c = CIRC_SPACE_TO_END(head, tail, XMIT_SIZE);
if (n < c) c=n;
memcpy(&iforce->xmit.buf[head],
data,
c);
if (n != c) {
memcpy(&iforce->xmit.buf[0],
data + c,
n - c);
}
XMIT_INC(head, n);
spin_unlock_irqrestore(&iforce->xmit_lock, flags);
/*
* If necessary, start the transmission
*/
switch (iforce->bus) {
#ifdef CONFIG_JOYSTICK_IFORCE_232
case IFORCE_232:
if (empty)
iforce_serial_xmit(iforce);
break;
#endif
#ifdef CONFIG_JOYSTICK_IFORCE_USB
case IFORCE_USB:
if (iforce->usbdev && empty &&
!test_and_set_bit(IFORCE_XMIT_RUNNING, iforce->xmit_flags)) {
iforce_usb_xmit(iforce);
}
break;
#endif
}
return 0;
}
void iforce_process_packet(struct iforce *iforce, u16 cmd, unsigned char *data)
{
struct input_dev *dev = iforce->dev;
int i;
static int being_used = 0;
if (being_used)
warn("re-entrant call to iforce_process %d", being_used);
being_used++;
#ifdef CONFIG_JOYSTICK_IFORCE_232
if (HI(iforce->expect_packet) == HI(cmd)) {
iforce->expect_packet = 0;
iforce->ecmd = cmd;
memcpy(iforce->edata, data, IFORCE_MAX_LENGTH);
}
#endif
wake_up(&iforce->wait);
if (!iforce->type) {
being_used--;
return;
}
switch (HI(cmd)) {
case 0x01: /* joystick position data */
case 0x03: /* wheel position data */
if (HI(cmd) == 1) {
input_report_abs(dev, ABS_X, (__s16) (((__s16)data[1] << 8) | data[0]));
input_report_abs(dev, ABS_Y, (__s16) (((__s16)data[3] << 8) | data[2]));
input_report_abs(dev, ABS_THROTTLE, 255 - data[4]);
if (LO(cmd) >= 8 && test_bit(ABS_RUDDER ,dev->absbit))
input_report_abs(dev, ABS_RUDDER, (__s8)data[7]);
} else {
input_report_abs(dev, ABS_WHEEL, (__s16) (((__s16)data[1] << 8) | data[0]));
input_report_abs(dev, ABS_GAS, 255 - data[2]);
input_report_abs(dev, ABS_BRAKE, 255 - data[3]);
}
input_report_abs(dev, ABS_HAT0X, iforce_hat_to_axis[data[6] >> 4].x);
input_report_abs(dev, ABS_HAT0Y, iforce_hat_to_axis[data[6] >> 4].y);
for (i = 0; iforce->type->btn[i] >= 0; i++)
input_report_key(dev, iforce->type->btn[i], data[(i >> 3) + 5] & (1 << (i & 7)));
/* If there are untouched bits left, interpret them as the second hat */
if (i <= 8) {
int btns = data[6];
if (test_bit(ABS_HAT1X, dev->absbit)) {
if (btns & 8) input_report_abs(dev, ABS_HAT1X, -1);
else if (btns & 2) input_report_abs(dev, ABS_HAT1X, 1);
else input_report_abs(dev, ABS_HAT1X, 0);
}
if (test_bit(ABS_HAT1Y, dev->absbit)) {
if (btns & 1) input_report_abs(dev, ABS_HAT1Y, -1);
else if (btns & 4) input_report_abs(dev, ABS_HAT1Y, 1);
else input_report_abs(dev, ABS_HAT1Y, 0);
}
}
input_sync(dev);
break;
case 0x02: /* status report */
input_report_key(dev, BTN_DEAD, data[0] & 0x02);
input_sync(dev);
/* Check if an effect was just started or stopped */
i = data[1] & 0x7f;
if (data[1] & 0x80) {
if (!test_and_set_bit(FF_CORE_IS_PLAYED, iforce->core_effects[i].flags)) {
/* Report play event */
input_report_ff_status(dev, i, FF_STATUS_PLAYING);
}
} else if (test_and_clear_bit(FF_CORE_IS_PLAYED, iforce->core_effects[i].flags)) {
/* Report stop event */
input_report_ff_status(dev, i, FF_STATUS_STOPPED);
}
if (LO(cmd) > 3) {
int j;
for (j = 3; j < LO(cmd); j += 2)
mark_core_as_ready(iforce, data[j] | (data[j+1]<<8));
}
break;
}
being_used--;
}
void
Ceval(Node *tp)
{
unsigned int i1, i2;
Node * tp0;
if(tp->Cone || tp->Czero) return;
switch(tp->code) {
case 0:
return;
case ELIST:
if(tp->t1 != 0)
Ceval(tp->t1);
if(tp->t2 != 0)
Ceval(tp->t2);
tp->Czero = tp->Cone = 0;
return;
case ASSIGN:
Ceval(tp->t2);
tp->Czero = tp->t2->Czero;
tp->Cone = tp->t2->Cone;
return;
case DONTCARE:
Ceval(tp->t2);
tp->Czero = tp->t2->Czero;
tp->Cone = tp->t2->Cone;
return;
case AND:
Ceval(tp->t2);
Ceval(tp->t1);
tp->Czero = tp->t2->Czero | tp->t1->Czero;
tp->Cone = tp->t2->Cone & tp->t1->Cone;
return;
case OR:
Ceval(tp->t2);
Ceval(tp->t1);
tp->Czero = tp->t2->Czero & tp->t1->Czero;
tp->Cone = tp->t2->Cone | tp->t1->Cone;
return;
case LAND:
Ceval(tp->t2);
Ceval(tp->t1);
tp->Czero = (~0 << 1) | ((tp->t2->Czero == ~0) || (tp->t1->Czero == ~0));
tp->Cone = (tp->t1->Cone) && (tp->t2->Cone);
return;
case LOR:
Ceval(tp->t2);
Ceval(tp->t1);
tp->Czero = (~0 << 1) | ((tp->t2->Czero == ~0) && (tp->t1->Czero == ~0));
tp->Cone = (tp->t1->Cone) ||(tp->t2->Cone);
return;
case NEG:
Ceval(tp->t1);
tp->Cone = tp->Czero = 0;
if(ISCONST(tp->t1))
tp->Czero = ~(tp->Cone = -tp->t1->Cone);
return;
case XOR:
Ceval(tp->t2);
Ceval(tp->t1);
tp->Czero = (tp->t2->Czero & tp->t1->Czero)
| (tp->t2->Cone & tp->t1->Cone);
tp->Cone = (tp->t2->Czero & tp->t1->Cone)
| (tp->t2->Cone & tp->t1->Czero);
return;
case FLONE:
if(!vconst(tp->t1)) {
fprintf(stderr, "Restrick hasn't done find left one for variable\n");
exits("error");;
}
tp->Cone = eval(tp->t1);
tp->Czero = ~tp->Cone;
return;
case FRONE:
if(!vconst(tp->t1)) {
fprintf(stderr, "Restrick hasn't done find right one for variable\n");
exits("error");;
}
tp->Cone = eval(tp->t1);
tp->Czero = ~tp->Cone;
return;
case GREY:
if(!vconst(tp->t1)) {
fprintf(stderr, "Restrick hasn't done grey code for variable\n");
exits("error");;
}
tp->Cone = eval(tp->t1);
tp->Czero = ~tp->Cone;
return;
case NOT:
Ceval(tp->t1);
tp->Cone = (tp->t1->Czero == ~0);
tp->Czero = (tp->t1->Cone) ? ~0 : (~0 << 1);
return;
case COM:
Ceval(tp->t1);
tp->Cone = (tp->t1->Czero);
tp->Czero = (tp->t1->Cone);
return;
case ADD:
Ceval(tp->t2);
Ceval(tp->t1);
i1 = tp->t1->Cone | tp->t1->Czero;
i1 = i1 & ~(i1 + 1);
i2 = tp->t2->Cone | tp->t2->Czero;
i2 = i2 & ~(i2 + 1);
tp->Cone = i1 & i2 & (tp->t1->Cone + tp->t2->Cone);
tp->Czero = i1 & i2 & ~tp->Cone;
for(i1 = 0; ; i1 = i1<<1 | 1)
if(((tp->t1->Czero | i1) == ~0)
&& ((tp->t2->Czero | i1) == ~0)) break;
tp->Czero |= ~(i1<<1 | 1);
return;
case SUB:
Ceval(tp->t2);
Ceval(tp->t1);
i1 = tp->t1->Cone | tp->t1->Czero;
i1 = i1 & ~(i1 + 1);
i2 = tp->t2->Cone | tp->t2->Czero;
i2 = i2 & ~(i2 + 1);
tp->Cone = i1 & i2 & (tp->t1->Cone + tp->t2->Cone);
tp->Czero = i1 & i2 & ~tp->Cone;
for(i1 = 0; ; i1 = i1<<1 | 1)
if(((tp->t1->Czero | i1) == ~0)
&& ((tp->t2->Czero | i1) == ~0)) break;
i2 = i1 ^ i1>>1;
if((tp->t1->Cone & i2) && (tp->t2->Czero & i2))
tp->Czero |= ~(i1<<1 | 1);
else if((tp->t1->Czero & i2) && (tp->t2->Cone & i2))
tp->Cone |= ~(i1<<1 | 1);
return;
case MUL:
if(!vconst(tp->t1)) {
fprintf(stderr, "Restrick hasn't done mult for variable\n");
exits("error");;
}
if(!vconst(tp->t2)) {
fprintf(stderr, "Restrick hasn't done mult for variable\n");
exits("error");;
}
tp->Cone = eval(tp->t1) * eval(tp->t2);
tp->Czero = ~tp->Cone;
return;
case DIV:
if(!vconst(tp->t1)) {
fprintf(stderr, "Restrick hasn't done div for variable\n");
exits("error");;
}
if(!vconst(tp->t2)) {
fprintf(stderr, "Restrick hasn't done div for variable\n");
exits("error");;
}
tp->Cone = eval(tp->t1) / eval(tp->t2);
tp->Czero = ~tp->Cone;
return;
case MOD:
if(!vconst(tp->t1)) {
fprintf(stderr, "Restrick hasn't done mod for variable\n");
exits("error");;
}
if(!vconst(tp->t2)) {
fprintf(stderr, "Restrick hasn't done mod for variable\n");
exits("error");;
}
tp->Cone = eval(tp->t1) % eval(tp->t2);
tp->Czero = ~tp->Cone;
return;
case GT:
case LT:
case GE:
case LE:
Ceval(tp->t2);
Ceval(tp->t1);
for(i1 = 0; ; i1 = i1<<1 | 1)
if(((tp->t1->Czero | tp->t1->Cone | i1) == ~0)
&& ((tp->t2->Czero | tp->t2->Cone | i1) == ~0)) break;
i1 = ~i1;
switch(tp->code) {
case LT:
case LE:
tp->Cone = (tp->t1->Cone & i1) < (tp->t2->Cone & i1);
tp->Czero = (tp->t1->Cone & i1) > (tp->t2->Cone & i1);
break;
case GT:
case GE:
tp->Cone = (tp->t1->Cone & i1) > (tp->t2->Cone & i1);
tp->Czero = (tp->t1->Cone & i1) < (tp->t2->Cone & i1);
}
return;
case EQ:
Ceval(tp->t2);
Ceval(tp->t1);
if(ISCONST(tp->t1) && ISCONST(tp->t2)) {
tp->Cone = (tp->t1->Cone == tp->t2->Cone) ? 1 : 0;
tp->Czero = ~tp->Cone;
}
else {
i1 = (tp->t1->Cone | tp->t1->Czero)
& (tp->t2->Cone | tp->t2->Czero);
tp->Czero = (i1 & (tp->t1->Cone ^ tp->t2->Cone)) ?
~0 : ~1;
tp->Cone = 0;
}
return;
case NE:
Ceval(tp->t2);
Ceval(tp->t1);
if(ISCONST(tp->t1) && ISCONST(tp->t2)) {
tp->Cone = (tp->t1->Cone != tp->t2->Cone) ? 1 : 0;
tp->Czero = ~tp->Cone;
}
else {
tp->Czero = ~1;
i1 = (tp->t1->Cone | tp->t1->Czero)
& (tp->t2->Cone | tp->t2->Czero);
tp->Cone = (i1 & (tp->t1->Cone ^ tp->t2->Cone)) ?
1 : 0;
}
return;
case LS:
Ceval(tp->t2);
Ceval(tp->t1);
if(ISCONST(tp->t2)) {
tp->Cone = tp->t1->Cone << tp->t2->Cone;
tp->Czero = tp->t1->Czero << tp->t2->Cone;
i1 = ~0 << tp->t2->Cone;
tp->Czero |= ~i1;
return;
}
tp->Cone = tp->Czero = 0;
return;
case RS:
Ceval(tp->t2);
Ceval(tp->t1);
if(ISCONST(tp->t2)) {
tp->Cone = tp->t1->Cone >> tp->t2->Cone;
tp->Czero = tp->t1->Czero >> tp->t2->Cone;
i1 = ~0 >> tp->t2->Cone;
tp->Czero |= ~i1;
return;
}
tp->Cone = tp->Czero = 0;
return;
case CND:
Ceval(tp->t1);
Ceval(tp->t2->t1);
Ceval(tp->t2->t2);
if(ISCONST(tp->t1)) {
if (tp->t1->Cone) {
tp->Cone = tp->t2->t1->Cone;
tp->Czero = tp->t2->t1->Czero;
return;
}
tp->Cone = tp->t2->t2->Cone;
tp->Czero = tp->t2->t2->Czero;
return;
}
tp->Cone = tp->t2->t1->Cone & tp->t2->t2->Cone;
tp->Czero = tp->t2->t1->Czero & tp->t2->t2->Czero;
return;
case SWITCH:
{
int o, z;
if(vconst(tp)) {
tp->Cone = eval(tp);
tp->Czero = ~tp->Cone;
return;
}
Ceval(tp->t1);
tp0 = tp;
o = z = ~0;
for(tp = tp->t2; tp; tp = tp->t2) {
if(tp->code == ALT) {
if(tp->t1) {
Ceval(tp->t1);
continue;
}
tp = tp->t2;
if(tp == 0)
break;
if(tp->code != CASE)
break;
Ceval(tp->t1);
o &= tp->t1->Cone;
z &= tp->t1->Czero;
continue;
}
if(tp->code != CASE)
break;
Ceval(tp->t1);
o &= tp->t1->Cone;
z &= tp->t1->Czero;
}
tp0->Cone = o;
tp0->Czero = z;
return;
}
case EQN:
tp0 = ((Hshtab *) (tp->t1))->assign;
Ceval(tp0);
tp->Cone = tp0->Cone;
tp->Czero = tp0->Czero;
return;
case BOTH:
case INPUT:
tp->Czero = ~1;
tp->Cone = 0;
return;
case FIELD:
i1 = HI((Hshtab *) tp->t1) - LO((Hshtab *) tp->t1);
tp->Cone = 0;
tp->Czero = (i1 > 31) ? 0 : ( ~0 << i1);
return;
case NUMBER:
tp->Cone = (int) tp->t1;
tp->Czero = ~tp->Cone;
return;
default:
fprintf(stderr,"unknown Ceval op %d\n", tp->code);
exits("error");;
}
void xqueue_push16(xqueue_t * const queue, const uint16_t value){
xqueue_push8(queue, HO(value));
xqueue_push8(queue, LO(value));
}
DEFINE_LED( 2, 2, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 2, 1, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 2, 0, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 1, 0, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 0, 0, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 0, 1, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 0, 2, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 1, 2, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0),
DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0),
};
#else
static const ledConfig_t defaultLedStripConfig[] = {
DEFINE_LED(15, 15, 0, LD(SOUTH) | LD(EAST), LF(ARM_STATE), LO(INDICATOR), 0),
DEFINE_LED(15, 8, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0),
DEFINE_LED(15, 7, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0),
DEFINE_LED(15, 0, 0, LD(NORTH) | LD(EAST), LF(ARM_STATE) , LO(INDICATOR), 0),
DEFINE_LED( 8, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0),
DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0),
DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0),
DEFINE_LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0),
DEFINE_LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0),
DEFINE_LED( 0, 15, 0, LD(SOUTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0),
/*
* local optimizations, most of which are probably
* machine independent
*/
NODE *
optim(NODE *p)
{
int o, ty;
NODE *sp, *q;
OFFSZ sz;
int i;
if (odebug) return(p);
ty = coptype(p->n_op);
if( ty == LTYPE ) return(p);
if( ty == BITYPE ) p->n_right = optim(p->n_right);
p->n_left = optim(p->n_left);
/* collect constants */
again: o = p->n_op;
switch(o){
case SCONV:
if (concast(p->n_left, p->n_type)) {
q = p->n_left;
nfree(p);
p = q;
break;
}
/* FALLTHROUGH */
case PCONV:
if (p->n_type != VOID)
p = clocal(p);
break;
case FORTCALL:
p->n_right = fortarg( p->n_right );
break;
case ADDROF:
if (LO(p) == TEMP)
break;
if( LO(p) != NAME ) cerror( "& error" );
if( !andable(p->n_left) && !statinit)
break;
LO(p) = ICON;
setuleft:
/* paint over the type of the left hand side with the type of the top */
p->n_left->n_type = p->n_type;
p->n_left->n_df = p->n_df;
p->n_left->n_ap = p->n_ap;
q = p->n_left;
nfree(p);
p = q;
break;
case NOT:
case UMINUS:
case COMPL:
if (LCON(p) && conval(p->n_left, o, p->n_left))
p = nfree(p);
break;
case UMUL:
/* Do not discard ADDROF TEMP's */
if (LO(p) == ADDROF && LO(p->n_left) != TEMP) {
q = p->n_left->n_left;
nfree(p->n_left);
nfree(p);
p = q;
break;
}
if( LO(p) != ICON ) break;
LO(p) = NAME;
goto setuleft;
case RS:
if (LCON(p) && RCON(p) && conval(p->n_left, o, p->n_right))
goto zapright;
sz = tsize(p->n_type, p->n_df, p->n_ap);
if (LO(p) == RS && RCON(p->n_left) && RCON(p) &&
(RV(p) + RV(p->n_left)) < sz) {
/* two right-shift by constants */
RV(p) += RV(p->n_left);
p->n_left = zapleft(p->n_left);
}
#if 0
else if (LO(p) == LS && RCON(p->n_left) && RCON(p)) {
RV(p) -= RV(p->n_left);
if (RV(p) < 0)
o = p->n_op = LS, RV(p) = -RV(p);
p->n_left = zapleft(p->n_left);
}
#endif
if (RO(p) == ICON) {
if (RV(p) < 0) {
RV(p) = -RV(p);
p->n_op = LS;
goto again;
}
#ifdef notyet /* must check for side effects, --a >> 32; */
if (RV(p) >= tsize(p->n_type, p->n_df, p->n_sue) &&
ISUNSIGNED(p->n_type)) { /* ignore signed shifts */
/* too many shifts */
tfree(p->n_left);
nfree(p->n_right);
p->n_op = ICON; p->n_lval = 0; p->n_sp = NULL;
} else
#endif
/* avoid larger shifts than type size */
if (RV(p) >= sz) {
RV(p) = RV(p) % sz;
werror("shift larger than type");
}
if (RV(p) == 0)
p = zapleft(p);
}
break;
case LS:
if (LCON(p) && RCON(p) && conval(p->n_left, o, p->n_right))
goto zapright;
sz = tsize(p->n_type, p->n_df, p->n_ap);
if (LO(p) == LS && RCON(p->n_left) && RCON(p)) {
/* two left-shift by constants */
RV(p) += RV(p->n_left);
p->n_left = zapleft(p->n_left);
}
#if 0
else if (LO(p) == RS && RCON(p->n_left) && RCON(p)) {
RV(p) -= RV(p->n_left);
p->n_left = zapleft(p->n_left);
}
#endif
if (RO(p) == ICON) {
if (RV(p) < 0) {
RV(p) = -RV(p);
p->n_op = RS;
goto again;
}
#ifdef notyet /* must check for side effects */
if (RV(p) >= tsize(p->n_type, p->n_df, p->n_sue)) {
/* too many shifts */
tfree(p->n_left);
nfree(p->n_right);
p->n_op = ICON; p->n_lval = 0; p->n_sp = NULL;
} else
#endif
/* avoid larger shifts than type size */
if (RV(p) >= sz) {
RV(p) = RV(p) % sz;
werror("shift larger than type");
}
if (RV(p) == 0)
p = zapleft(p);
}
break;
case MINUS:
if (LCON(p) && RCON(p) && p->n_left->n_sp == p->n_right->n_sp) {
/* link-time constants, but both are the same */
/* solve it now by forgetting the symbols */
p->n_left->n_sp = p->n_right->n_sp = NULL;
}
if( !nncon(p->n_right) ) break;
RV(p) = -RV(p);
o = p->n_op = PLUS;
case MUL:
/*
* Check for u=(x-y)+z; where all vars are pointers to
* the same struct. This has two advantages:
* 1: avoid a mul+div
* 2: even if not allowed, people may get surprised if this
* calculation do not give correct result if using
* unaligned structs.
*/
if (p->n_type == INTPTR && RCON(p) &&
LO(p) == DIV && RCON(p->n_left) &&
RV(p) == RV(p->n_left) &&
LO(p->n_left) == MINUS) {
q = p->n_left->n_left;
if (q->n_left->n_type == PTR+STRTY &&
q->n_right->n_type == PTR+STRTY &&
strmemb(q->n_left->n_ap) ==
strmemb(q->n_right->n_ap)) {
p = zapleft(p);
p = zapleft(p);
}
}
/* FALLTHROUGH */
case PLUS:
case AND:
case OR:
case ER:
/* commutative ops; for now, just collect constants */
/* someday, do it right */
if( nncon(p->n_left) || ( LCON(p) && !RCON(p) ) )
SWAP( p->n_left, p->n_right );
/* make ops tower to the left, not the right */
if( RO(p) == o ){
NODE *t1, *t2, *t3;
t1 = p->n_left;
sp = p->n_right;
t2 = sp->n_left;
t3 = sp->n_right;
/* now, put together again */
p->n_left = sp;
sp->n_left = t1;
sp->n_right = t2;
sp->n_type = p->n_type;
p->n_right = t3;
}
if(o == PLUS && LO(p) == MINUS && RCON(p) && RCON(p->n_left) &&
conval(p->n_right, MINUS, p->n_left->n_right)){
zapleft:
q = p->n_left->n_left;
nfree(p->n_left->n_right);
nfree(p->n_left);
p->n_left = q;
}
if( RCON(p) && LO(p)==o && RCON(p->n_left) &&
conval( p->n_right, o, p->n_left->n_right ) ){
goto zapleft;
}
else if( LCON(p) && RCON(p) && conval( p->n_left, o, p->n_right ) ){
zapright:
nfree(p->n_right);
q = makety(p->n_left, p->n_type, p->n_qual,
p->n_df, p->n_ap);
nfree(p);
p = clocal(q);
break;
}
/* change muls to shifts */
if( o == MUL && nncon(p->n_right) && (i=ispow2(RV(p)))>=0){
if( i == 0 ) { /* multiplication by 1 */
goto zapright;
}
o = p->n_op = LS;
p->n_right->n_type = INT;
p->n_right->n_df = NULL;
RV(p) = i;
}
/* change +'s of negative consts back to - */
if( o==PLUS && nncon(p->n_right) && RV(p)<0 ){
RV(p) = -RV(p);
o = p->n_op = MINUS;
}
/* remove ops with RHS 0 */
if ((o == PLUS || o == MINUS || o == OR || o == ER) &&
nncon(p->n_right) && RV(p) == 0) {
goto zapright;
}
break;
case DIV:
if( nncon( p->n_right ) && p->n_right->n_lval == 1 )
goto zapright;
if (LCON(p) && RCON(p) && conval(p->n_left, DIV, p->n_right))
goto zapright;
if (RCON(p) && ISUNSIGNED(p->n_type) && (i=ispow2(RV(p))) > 0) {
p->n_op = RS;
RV(p) = i;
q = p->n_right;
if(tsize(q->n_type, q->n_df, q->n_ap) > SZINT)
p->n_right = makety(q, INT, 0, 0, 0);
break;
}
break;
case MOD:
if (RCON(p) && ISUNSIGNED(p->n_type) && ispow2(RV(p)) > 0) {
p->n_op = AND;
RV(p) = RV(p) -1;
break;
}
break;
case EQ:
case NE:
case LT:
case LE:
case GT:
case GE:
case ULT:
case ULE:
case UGT:
case UGE:
if( !LCON(p) ) break;
/* exchange operands */
sp = p->n_left;
p->n_left = p->n_right;
p->n_right = sp;
p->n_op = revrel[p->n_op - EQ ];
break;
#ifdef notyet
case ASSIGN:
/* Simple test to avoid two branches */
if (RO(p) != NE)
break;
q = p->n_right;
if (RCON(q) && RV(q) == 0 && LO(q) == AND &&
RCON(q->n_left) && (i = ispow2(RV(q->n_left))) &&
q->n_left->n_type == INT) {
q->n_op = RS;
RV(q) = i;
}
break;
#endif
}
return(p);
}
static int readStationData (WVIEWD_WORK *work)
{
int i, retVal, groupType, checkSum, channel;
float tempFloat;
WMR918_IF_DATA* ifWorkData = (WMR918_IF_DATA*)work->stationData;
UCHAR *pPacket = &wmr918Work.readData[2];
// read the first three bytes -- 0xff, 0xff, <type>
retVal = (*work->medium.read) (&work->medium, &wmr918Work.readData[0], 3, WMR918_READ_TIMEOUT);
if (retVal != 3)
{
radMsgLog (PRI_MEDIUM, "readStationData: read header failed: %s",
strerror(errno));
emailAlertSend(ALERT_TYPE_STATION_READ);
return ERROR;
}
while ((wmr918Work.readData[0] != 0xff) ||
(wmr918Work.readData[1] != 0xff) ||
(wmr918Work.readData[2] > 15))
{
wmr918Work.readData[0] = wmr918Work.readData[1];
wmr918Work.readData[1] = wmr918Work.readData[2];
retVal = (*work->medium.read) (&work->medium, &wmr918Work.readData[2], 1, WMR918_READ_TIMEOUT);
if (retVal != 1)
{
radMsgLog (PRI_MEDIUM, "readStationData: read header X failed: %s",
strerror(errno));
emailAlertSend(ALERT_TYPE_STATION_READ);
return ERROR;
}
}
groupType = (int)wmr918Work.readData[2];
// read remaining bytes of this type + checksum:
retVal = (*work->medium.read) (&work->medium,
&wmr918Work.readData[3],
wmr918GroupLength[groupType],
WMR918_READ_TIMEOUT);
if (retVal != wmr918GroupLength[groupType])
{
radMsgLog (PRI_MEDIUM, "readStationData: read payload failed: %s",
strerror(errno));
emailAlertSend(ALERT_TYPE_STATION_READ);
return ERROR;
}
// Verify checksum:
checkSum = 0;
for (i = 0; i < wmr918GroupLength[groupType] + 2; i ++)
{
checkSum += wmr918Work.readData[i];
}
checkSum &= 0xFF;
if (checkSum != wmr918Work.readData[wmr918GroupLength[groupType] + 2])
{
radMsgLog (PRI_MEDIUM, "readStationData: checksum mismatch: computed %2.2X, RX %2.2X",
checkSum, wmr918Work.readData[wmr918GroupLength[groupType] + 2]);
emailAlertSend(ALERT_TYPE_STATION_READ);
return ERROR;
}
// Parse it for data:
switch (groupType)
{
case WMR918GROUP0:
{
wmr918Work.dataRXMask |= WMR918_SENSOR_WIND;
// Battery status:
wmr918Work.sensorData.windBatteryStatus = HI(pPacket[1]);
// Wind gust direction:
tempFloat = (float)(NUM(pPacket[2]) + (100 * LO(pPacket[3])));
wmr918Work.sensorData.maxWindDir = tempFloat;
wmr918Work.sensorData.windDir = wmr918Work.sensorData.maxWindDir;
// Gust speed:
tempFloat = (float)(HI(pPacket[3]));
tempFloat *= 0.1;
tempFloat += (float)(NUM(pPacket[4]));
tempFloat *= 2.237; // convert to mph
wmr918Work.sensorData.maxWindSpeed = tempFloat;
// Average speed:
tempFloat = (float)(NUM(pPacket[5]));
tempFloat *= 0.1;
tempFloat += (float)(LO(pPacket[6]) * 10);
tempFloat *= 2.237; // convert to mph
wmr918Work.sensorData.windSpeed = tempFloat;
break;
}
case WMR918GROUP1:
{
wmr918Work.dataRXMask |= WMR918_SENSOR_RAIN;
// Battery status:
wmr918Work.sensorData.rainBatteryStatus = HI(pPacket[1]);
// Rain rate:
tempFloat = (float)(NUM(pPacket[2]) + (100 * LO(pPacket[3])));
tempFloat *= 0.03937;
wmr918Work.sensorData.rainrate = tempFloat;
// Rain total:
tempFloat = (float)NUM(pPacket[4]);
tempFloat += (float)(NUM(pPacket[5]) * 100);
tempFloat *= 0.03937;
wmr918Work.sensorData.rain = tempFloat;
break;
}
case WMR918GROUP2:
{
// Up to 3 channels for WMR968, capture value
channel = LO(pPacket[1]);
// verify channel and set to use as index:
if (channel != 0x4 && channel != 0x2 && channel != 0x1)
{
radMsgLog(PRI_MEDIUM, "readStationData: group2 data has invalid channel %d", channel);
break;
}
if (channel == 0x4)
{
channel = 3;
}
if ((int)ifWorkData->outsideChannel == channel)
wmr918Work.dataRXMask |= WMR918_SENSOR_OUT_TEMP;
// Battery status:
if ((int)ifWorkData->outsideChannel == channel)
wmr918Work.sensorData.outTempBatteryStatus = HI(pPacket[1]) & 0x7;
else if ((int)ifWorkData->outsideChannel < channel)
wmr918Work.sensorData.extraBatteryStatus[channel - 1] = HI(pPacket[1]);
else
wmr918Work.sensorData.extraBatteryStatus[channel] = HI(pPacket[1]);
// Temp:
tempFloat = (float)NUM(pPacket[2]);
tempFloat *= 0.1;
tempFloat += (float)(LO(pPacket[3]) * 10);
if (BIT(HI(pPacket[3]),3))
{
tempFloat *= -1.0;
}
tempFloat *= 9;
tempFloat /= 5;
tempFloat += 32;
if ((int)ifWorkData->outsideChannel == channel)
wmr918Work.sensorData.outTemp = tempFloat;
else if ((int)ifWorkData->outsideChannel < channel)
wmr918Work.sensorData.extraTemp[channel - 1] = tempFloat;
else
wmr918Work.sensorData.extraTemp[channel] = tempFloat;
// Humidity:
tempFloat = (float)NUM(pPacket[4]);
if ((int)ifWorkData->outsideChannel == channel)
wmr918Work.sensorData.outHumidity = tempFloat;
else if ((int)ifWorkData->outsideChannel < channel)
wmr918Work.sensorData.extraHumidity[channel - 1] = tempFloat;
else
wmr918Work.sensorData.extraHumidity[channel] = tempFloat;
break;
}
case WMR918GROUP3:
{
// check if this is the primary outside temperature selection
// if not then this sensor will become channel 0 extra sensor
if ((int)ifWorkData->outsideChannel == 0)
wmr918Work.dataRXMask |= WMR918_SENSOR_OUT_TEMP;
// Battery status:
if ((int)ifWorkData->outsideChannel == 0)
wmr918Work.sensorData.outTempBatteryStatus = HI(pPacket[1]) & 0x7;
else
wmr918Work.sensorData.extraBatteryStatus[0] = HI(pPacket[1]);
// Temp:
tempFloat = (float)NUM(pPacket[2]);
tempFloat *= 0.1;
tempFloat += (float)(LO(pPacket[3]) * 10);
if (BIT(HI(pPacket[3]),3))
{
tempFloat *= -1.0;
}
tempFloat *= 9;
tempFloat /= 5;
tempFloat += 32;
if ((int)ifWorkData->outsideChannel == 0)
wmr918Work.sensorData.outTemp = tempFloat;
else
wmr918Work.sensorData.extraTemp[0] = tempFloat;
// Humidity:
tempFloat = (float)NUM(pPacket[4]);
if ((int)ifWorkData->outsideChannel == 0)
wmr918Work.sensorData.outHumidity = tempFloat;
else
wmr918Work.sensorData.extraHumidity[0] = tempFloat;
break;
}
case WMR918GROUP4:
{
int tmpChan;
//
// Channel is bit-position encoded in the low order of packet 1 range 1-3
//
tmpChan = LO(pPacket[1]) & 0x07;
if (tmpChan == 4)
{
tmpChan = 3;
}
// Sanity -- keeping same.
if ((tmpChan <= 0) || (tmpChan > 4))
{
tmpChan = 1;
}
// Temp:
tempFloat = (float)NUM(pPacket[2]);
tempFloat *= 0.1;
tempFloat += (float)(LO(pPacket[3]) * 10);
if (BIT(HI(pPacket[3]),3))
{
tempFloat *= -1.0;
}
tempFloat *= 9;
tempFloat /= 5;
tempFloat += 32;
//
// To support legacy pool sensors take channel 1 (note - channel selection on the
// pool sensor is by switch on some models.
//
if (tmpChan == 1)
{
wmr918Work.sensorData.poolTempBatteryStatus = HI (pPacket[1]) & 0x7;
wmr918Work.sensorData.pool = tempFloat;
}
wmr918Work.sensorData.extraTemp[tmpChan - 1] = tempFloat;
break;
}
case WMR918GROUP5:
{
wmr918Work.dataRXMask |= WMR918_SENSOR_IN_TEMP;
// Battery status:
wmr918Work.sensorData.inTempBatteryStatus = HI(pPacket[1]);
// Temp:
tempFloat = (float)NUM(pPacket[2]);
tempFloat *= 0.1;
tempFloat += (float)(LO(pPacket[3]) * 10);
if (BIT(HI(pPacket[3]),3))
{
tempFloat *= -1.0;
}
tempFloat *= 9;
tempFloat /= 5;
tempFloat += 32;
wmr918Work.sensorData.inTemp = tempFloat;
// Humidity:
tempFloat = (float)NUM(pPacket[4]);
wmr918Work.sensorData.inHumidity = tempFloat;
// BP:
tempFloat = (float)pPacket[6];
tempFloat += 795;
tempFloat *= 0.02953;
wmr918Work.sensorData.pressure = tempFloat;
break;
}
case WMR918GROUP6:
{
wmr918Work.dataRXMask |= WMR918_SENSOR_IN_TEMP;
// Battery status:
wmr918Work.sensorData.inTempBatteryStatus = HI(pPacket[1]);
// Temp:
tempFloat = (float)NUM(pPacket[2]);
tempFloat *= 0.1;
tempFloat += (float)(LO(pPacket[3]) * 10);
if (BIT(HI(pPacket[3]),3))
{
tempFloat *= -1.0;
}
tempFloat *= 9;
tempFloat /= 5;
tempFloat += 32;
wmr918Work.sensorData.inTemp = tempFloat;
// Humidity:
tempFloat = (float)NUM(pPacket[4]);
wmr918Work.sensorData.inHumidity = tempFloat;
// BP:
tempFloat = (float)((int)pPacket[6] + (int)(BIT(LO(pPacket[7]),0) << 8));
tempFloat += 600;
tempFloat *= 0.02953;
wmr918Work.sensorData.pressure = tempFloat;
// Forecast:
wmr918Work.sensorData.tendency = HI(pPacket[7]);
break;
}
default:
{
break;
}
}
return groupType;
}
void led_init() {
PINMODE(LEDY_DDR, LEDY,OUTPUT);
LO(LEDY_PORT,LEDY);
PINMODE(LEDG_DDR, LEDG,OUTPUT);
LO(LEDG_PORT,LEDG);
}
int ipx_int7a(void)
{
u_short port; /* port here means DOS IPX socket */
u_short newPort;
u_char *AddrPtr;
far_t ECBPtr;
unsigned long network;
int hops, ticks;
n_printf("IPX: request number 0x%x\n", LWORD(ebx));
switch (LWORD(ebx)) {
case IPX_OPEN_SOCKET:
if (LO(ax) != 0xff)
n_printf("IPX: OpenSocket: longevity flag (%#x) not supported\n", LO(ax));
port = LWORD(edx);
newPort = 0;
LO(ax) = IPXOpenSocket(port, &newPort);
if (LO(ax) == RCODE_SUCCESS)
LWORD(edx) = newPort;
break;
case IPX_CLOSE_SOCKET:
port = LWORD(edx);
LO(ax) = IPXCloseSocket(port);
break;
case IPX_GET_LOCAL_TARGET:
/* do nothing here because routing is handled by IPX */
/* normally this would return an ImmediateAddress, but */
/* the ECB ImmediateAddress is never used, so just return */
network = READ_DWORD(SEGOFF2LINEAR(REG(es), LWORD(esi)));
n_printf("IPX: GetLocalTarget for network %08lx\n", network );
if( network==0 || memcmp(&network, MyAddress, 4) == 0 ) {
n_printf("IPX: returning GLT success for local address\n");
LO(ax) = RCODE_SUCCESS;
LWORD(ecx) = 1;
} else {
if( IPXGetLocalTarget( network, &hops, &ticks )==0 ) {
LO(ax) = RCODE_SUCCESS;
LWORD(ecx) = ticks;
} else {
n_printf("IPX: GetLocalTarget failed.\n");
LO(ax) = RCODE_CANNOT_FIND_ROUTE;
}
}
break;
case IPX_FAST_SEND:
n_printf("IPX: fast send\n");
/* just fall through to regular send */
case IPX_SEND_PACKET: {
int ret;
ECBPtr.segment = REG(es);
ECBPtr.offset = LWORD(esi);
n_printf("IPX: send packet ECB at %p\n", ECBp);
/* What the hell is the async send? Do it synchroniously! */
ret = IPXSendPacket(ECBPtr);
if ((ret == RCODE_SUCCESS) && FARt_PTR(ECBp->ESRAddress))
ipx_esr_call(ECBPtr, ESR_CALLOUT_IPX);
LO(ax) = ret;
break;
}
case IPX_LISTEN_FOR_PACKET:
ECBPtr.segment = REG(es);
ECBPtr.offset = LWORD(esi);
n_printf("IPX: listen for packet, ECB at %x:%x\n",
ECBPtr.segment, ECBPtr.offset);
/* put this packet on the queue of listens for this socket */
LO(ax) = IPXListenForPacket(ECBPtr);
break;
case IPX_SCHEDULE_IPX_EVENT:
ECBPtr.segment = REG(es);
ECBPtr.offset = LWORD(esi);
n_printf("IPX: schedule IPX event for ECB at %x:%x\n",
ECBPtr.segment, ECBPtr.offset);
/* put this packet on the queue of AES events for this socket */
LO(ax) = IPXScheduleEvent(ECBPtr, IU_ECB_IPX_WAITING,
LWORD(eax));
break;
case IPX_CANCEL_EVENT:
ECBPtr.segment = REG(es);
ECBPtr.offset = LWORD(esi);
n_printf("IPX: cancel event for ECB at %p\n", ECBp);
LO(ax) = IPXCancelEvent(ECBPtr);
break;
case IPX_SCHEDULE_AES_EVENT:
ECBPtr.segment = REG(es);
ECBPtr.offset = LWORD(esi);
n_printf("IPX: schedule AES event ECB at %p\n", FARt_PTR(ECBPtr));
/* put this packet on the queue of AES events for this socket */
LO(ax) = IPXScheduleEvent(ECBPtr, IU_ECB_AES_WAITING,
LWORD(eax));
break;
case IPX_GET_INTERVAL_MARKER:
/* Note that timerticks is actually an unsigned long in BIOS */
/* this works because of intel lo-hi architecture */
LWORD(eax) = READ_WORD(BIOS_TICK_ADDR);
n_printf("IPX: get interval marker %d\n", LWORD(eax));
/* n_printf("IPX: doing extra relinquish control\n"); */
IPXRelinquishControl();
break;
case IPX_GET_INTERNETWORK_ADDRESS:
n_printf("IPX: get internetwork address\n");
AddrPtr = SEG_ADR((u_char *), es, si);
memcpy(AddrPtr, MyAddress, 10);
break;
case IPX_RELINQUISH_CONTROL:
n_printf("IPX: relinquish control\n");
IPXRelinquishControl();
break;
case IPX_DISCONNECT:
n_printf("IPX: disconnect\n");
break;
case IPX_SHELL_HOOK:
n_printf("IPX: shell hook\n");
break;
case IPX_GET_MAX_PACKET_SIZE:
n_printf("IPX: get max packet size\n");
/* return max data size in AX, and suggested retries in CL */
/* DANG_FIXTHIS - return a real max packet size here */
LWORD(eax) = 1024; /* must be a power of 2 */
LO(cx) = 20;
break;
case IPX_GET_MEDIA_DATA_SIZE:
n_printf("IPX: get max packet size\n");
/* return max data size in AX, and suggested retries in CL */
/* DANG_FIXTHIS - return a real max media size here */
LWORD(eax) = 1480;
LO(cx) = 20;
break;
case IPX_CLEAR_SOCKET:
n_printf("IPX: clear socket\n");
break;
default:
n_printf("IPX: Unimplemented function.\n");
break;
}
return 1;
}
void led_y_off() { LO(LEDY_PORT,LEDY); }