int floppy_seek_track(uint_t drive, uint_t track)
{
if (drive >= MAX_DRIVES) {
return -1;
}
if (track == floppy_drives[drive].track) {
return 0;
}
floppy_motor_on(drive);
prepare_wait_irq(FLOPPY_IRQ);
floppy_command(SEEK);
floppy_command(drive);
floppy_command(track);
wait_irq(FLOPPY_IRQ);
floppy_drive_seeked = drive;
floppy_sense_interrupt();
if (track != floppy_drives[drive].track) {
kprintf("FDD: ERROR: fd%u couldn't seek\n", drive);
floppy_prepare_motor_off(drive);
return -2;
}
/*kprintf("FDD: fd%u: Seeked track succesfully\n", drive);*/
floppy_prepare_motor_off(drive);
return 0;
}
void floppy_reset(void)
{
int a;
prepare_wait_irq(FLOPPY_IRQ);
outportb((FLOPPY_FIRST + DIGITAL_OUTPUT_REGISTER), 0x00); /*disable controller*/
kwait(0, 1000 * 50);
outportb((FLOPPY_FIRST + DIGITAL_OUTPUT_REGISTER), 0x0c); /*enable controller*/
kprintf("FDD: Reseted controller\n");
wait_irq(FLOPPY_IRQ);
kprintf("FDD: Waited for it\n");
for(a = 0; a < 4; a++) {
floppy_sense_interrupt();
}
outportb(FLOPPY_FIRST + CONFIGURATION_CONTROL_REGISTER, 0);
floppy_configure();
if (floppy_drives[0].type) {
floppy_calibrate(0);
}
if (floppy_drives[1].type) {
floppy_calibrate(1);
}
kprintf("FDD: Calibrated drives\n");
}
void rgbled_init(void)
{
unsigned char value = 0xff;
unsigned char r,g,b;
r = g = b = 0x00;
rt_i2c_write(AXP209_DEVADDR, 0x82, &value, 1);
value = 0xE1;
rt_i2c_write(RGBLED_DEVADDR, 0x01, &value, 1);
value = 0x07;
rt_i2c_write(RGBLED_DEVADDR, 0x30, &value, 1);
value = 0x03;
rt_i2c_write(RGBLED_DEVADDR, 0x31, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x32, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x33, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x34, &b, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x35, &g, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x36, &r, 1);
rt_i2c_read(AXP209_DEVADDR, 0xB9, &value, 1);
value = value * 100 / 0x7f;
if(value < 5)
{
r = 255;
g = 0;
b = 0;
value = 0x13;
rt_i2c_write(RGBLED_DEVADDR, 0x33, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x34, &b, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x35, &g, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x36, &r, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x3f, &value, 1);
value = 0x80;
if(wait_irq())
rt_i2c_write(AXP209_DEVADDR, 0x32, &value, 1);
}
else if(value)
{
r = 255;
g = 156;
b = 00;
value = 0x13;
rt_i2c_write(RGBLED_DEVADDR, 0x31, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x32, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x33, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x34, &b, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x35, &g, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x36, &r, 1);
value = 0x13;
rt_i2c_write(RGBLED_DEVADDR, 0x37, &value, 1);
value = 0x03;
rt_i2c_write(RGBLED_DEVADDR, 0x3A, &value, 1);
rt_i2c_write(RGBLED_DEVADDR, 0x3D, &value, 1);
value = 0x00;
rt_i2c_write(RGBLED_DEVADDR, 0x3f, &value, 1);
}
printf("elec %x\n", value);
}
/*************************************************************************
* Function : mp3startCmd
*************************************************************************/
int
mp3startCmd(int cmd)
{
epics_busy = 1;
infXchgMP3[0] = cmd;
pnx0106_epics_interrupt_req();
wait_irq();
epics_busy = 0;
// return infXchgMP3[0] != 0;
return infXchgMP3[0];
}
void floppy_calibrate(uint_t drive)
{
if (drive >= MAX_DRIVES || !floppy_drives[drive].type) {
return;
}
prepare_wait_irq(FLOPPY_IRQ);
floppy_command(RECALIBRATE); /* (re)calibrate drive*/
floppy_command(drive);
wait_irq(FLOPPY_IRQ);
kprintf("FDD: fd%u calibrated\n", drive);
}
/*************************************************************************
* Function : mp3decContinue
*************************************************************************/
int
mp3decContinue(void)
{
if (epics_busy)
{
wait_irq();
epics_busy = 0;
}
// return infXchgMP3[0] != 0;
return infXchgMP3[0];
}
static void
dequeue_key(struct bregs *regs, int incr, int extended)
{
u16 buffer_head;
u16 buffer_tail;
for (;;) {
buffer_head = GET_BDA(kbd_buf_head);
buffer_tail = GET_BDA(kbd_buf_tail);
if (buffer_head != buffer_tail)
break;
if (!incr) {
regs->flags |= F_ZF;
return;
}
wait_irq();
}
u8 ascii_code = GET_FARVAR(SEG_BDA, *(u8*)(buffer_head+0));
u8 scan_code = GET_FARVAR(SEG_BDA, *(u8*)(buffer_head+1));
if ((ascii_code == 0xF0 && scan_code != 0)
|| (ascii_code == 0xE0 && !extended))
ascii_code = 0;
regs->ax = (scan_code << 8) | ascii_code;
if (!incr) {
regs->flags &= ~F_ZF;
return;
}
u16 buffer_start = GET_BDA(kbd_buf_start_offset);
u16 buffer_end = GET_BDA(kbd_buf_end_offset);
buffer_head += 2;
if (buffer_head >= buffer_end)
buffer_head = buffer_start;
SET_BDA(kbd_buf_head, buffer_head);
}
int floppy_rw_sector(uint_t drive, uint8_t sector, uint8_t head, uint8_t cylinder, uintptr_t buffer, int write)
{
int a;
alku:
if (drive >= MAX_DRIVES || !floppy_drives[drive].type) {
return -1;
}
if (inportb(FLOPPY_FIRST + DIGITAL_INPUT_REGISTER) & 0x80) {
/* disk was changed */
floppy_seek_track(drive, 1);
floppy_calibrate(drive);
floppy_motor_off(drive);
if (inportb(FLOPPY_FIRST + DIGITAL_INPUT_REGISTER) & 0x80) {
kprintf("FDD: No floppy in fd%u\n", drive);
return -2;
} else {
kprintf("FDD: Floppy changed, trying again...\n");
goto alku;
}
}
if (floppy_seek_track(drive, cylinder))
if (floppy_seek_track(drive, cylinder))
if (floppy_seek_track(drive, cylinder)) {
return -1; /* three seeks? */
}
/* floppy_seek_track actually starts motor already, but... */
floppy_motor_on(drive);
if (!(inportb(FLOPPY_FIRST + MAIN_STATUS_REGISTER) & 0x20)) {
panic("Non-dma floppy transfer?\n");
}
/* block size */
floppy_init_dma(buffer, 512, write);
kwait(0, 1000 * floppy_params.head_settle_time);
floppy_wait();
prepare_wait_irq(FLOPPY_IRQ);
floppy_command(write ? WRITE_DATA : READ_DATA);
floppy_command((head << 2) | drive);
floppy_command(cylinder);
floppy_command(head);
floppy_command(sector);
floppy_command(floppy_params.bytes_per_sector); /*sector size = 128*2^size*/
floppy_command(floppy_params.sectors_per_track); /*last sector*/
floppy_command(floppy_params.gap_length); /*27 default gap3 value*/
floppy_command(floppy_params.data_length); /*default value for data length*/
//kprintf("FDD: BPS: %u, SPT: %u, GL: %u, DL: %u\n", floppy_params.bytes_per_sector, floppy_params.sectors_per_track, floppy_params.gap_length, floppy_params.data_length);
wait_irq(FLOPPY_IRQ);
//kprintf("We got values ");
for (a = 0; a < 7; a++) { /* TODO: Put these values somewhere? */
floppy_wait_data();
inportb(FLOPPY_FIRST + DATA_FIFO);
//kprintf("%d ", inportb(FLOPPY_FIRST + DATA_FIFO));
}
//kprintf(" from floppy controller after reading\n");
floppy_prepare_motor_off(drive);
return 0;
}
// APM cpu idle
static void
handle_155305(struct bregs *regs)
{
wait_irq();
set_success(regs);
}
