+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <math.h>
+
+#include "rk05.h"
+#include "speed8.h"
+#include "link.h"
+#include "log.h"
+
+/* Commands for the RK8E controller */
+#define RK_CM_READ 00000
+#define RK_CM_SEEK_ONLY 03000
+#define RK_CM_WRITE 04000
+#define RK_CM_TRANS256 00000
+#define RK_CM_TRANS128 00100
+
+static char * bars(int width, int max, int current){
+ static char buffer[100];
+ if (width>80) width=80;
+ int i;
+ float width_f=width;
+ float max_f=max;
+ float current_f=current;
+ float step=width_f/max_f;
+ float dots=step*current_f;
+ int actual=round(dots);
+ buffer[0]='[';
+ buffer[width+1]=']';
+ for (i=0; i<width; i++){
+ if (i<actual) buffer[i+1]='*';
+ else buffer[i+1]=' ';
+ }
+ int perc=round((current_f/max_f)*100);
+ sprintf(buffer+width+2," %3i%%",perc);
+ return buffer;
+}
+
+int rk05_write_sector(uint8_t drive, uint16_t disk_address, uint16_t mem_address){
+ uint16_t cb[5];
+ debug("rk05_write_sector(): Drive %i, DA: %05o, CA: %04o\n",drive,disk_address, mem_address);
+ if ((disk_address>=203*2*16*256)||(mem_address>4095)||(drive>3)) {
+ debug("rk05_write_sector(): Invalid parameters!\n");
+ errno=EINVAL;
+ return -1;
+ }
+ cb[0]=ATTENTION;
+ cb[1]=CMD_RK_GO;
+ cb[2]=mem_address; // Current address
+ cb[3]=RK_CM_WRITE|(drive&3)<<1
+ | ((disk_address>>12)&1)
+ | RK_CM_TRANS256
+ | 010; // Field!!
+ cb[4]=disk_address&07777;
+ return link_write(cb,5);
+}
+
+int rk05_read_sector(uint8_t drive, uint16_t disk_address, uint16_t mem_address){
+ debug("rk05_read_sector(): Drive %i, DA: %05o, CA: %04o\n",drive,disk_address, mem_address);
+ uint16_t cb[5];
+ if ((disk_address>=RK05_TRACKS*16)||(mem_address>4095)||(drive>3)) {
+ debug("rk05_read_sector(): Invalid parameters!\n");
+ errno=EINVAL;
+ return -1;
+ }
+ cb[0]=ATTENTION;
+ cb[1]=CMD_RK_GO;
+ cb[2]=mem_address; // Current address
+ cb[3]=RK_CM_READ|(drive&3)<<1
+ | ((disk_address>>12)&1)
+ | 010; // Field!!
+ cb[4]=disk_address&07777;
+ link_write(cb,5);
+ return 0;
+}
+
+int rk05_write_track(uint8_t drive, uint16_t track){
+ uint16_t cb[4];
+ uint16_t sector=track*16+0;
+ debug("rk05_write_track(): Drive %i, Track: %i\n",drive,track);
+ if ((drive>3)||(track>=RK05_TRACKS)){
+ debug("rk05_write_track(): Invalid parameters!\n");
+ errno=EINVAL;
+ return -1;
+ }
+ cb[0]=ATTENTION;
+ cb[1]=CMD_RK_W_TRACK;
+ cb[2]=((drive&3)<<1)
+ |((sector>>12)&1);
+ cb[3]=(sector)&07777;
+ return link_write(cb,4);
+}
+
+int rk05_read_track(uint8_t drive, uint16_t track){
+ uint16_t cb[4];
+ uint16_t sector=track*16+0;
+ debug("rk05_read_track(): Drive %i, Track: %i\n",drive,track);
+ if ((drive>3)||(track>=RK05_TRACKS)){
+ debug("rk05_read_track(): Invalid parameters!\n");
+ errno=EINVAL;
+ return -1;
+ }
+ cb[0]=ATTENTION;
+ cb[1]=CMD_RK_R_TRACK;
+ cb[2]=((drive&3)<<1)
+ |((sector>>12)&1);
+ cb[3]=sector&07777;
+ return link_write(cb,4);
+}
+
+int rk05_status(uint16_t * status){
+ uint16_t cb[2];
+ int res;
+ debug("rk05_status()\n");
+ cb[0]=ATTENTION;
+ cb[1]=CMD_RK_STATUS;
+ res=link_write(cb,2);
+ if (res<0) return res;
+ res=link_read(cb,1);
+ if (res<0) return res;
+ if (status) *status=cb[0];
+ debug("rk05_status(): %04o\n",cb[0]);
+ return 0;
+}
+
+int rk05_recalibrate(uint8_t drive){
+ uint16_t cb[2];
+ debug("rk05_recalibrate(): Drive: %i\n",drive);
+ if (drive>3){
+ debug("rk05_recalibrate(): Invalid drive!\n");
+ errno=EINVAL;
+ return -1;
+ }
+ cb[0]=ATTENTION;
+ cb[1]=CMD_RK_RECAL0 + 0100*drive;
+ return link_write(cb,2);
+}
+
+
+int order1[16]={0, 2, 4, 6, 8, 10, 12, 14, 1,3,5,7,9,11,13,15};
+int order2[16]={0, 3, 6, 9, 12, 15, 1, 4, 7,10,13,2,5,8,11,14};
+int order3[16]={0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
+int order4[16]={0,4,8,12,1,5,9,13,2,6,10,14,3,7,11,15};
+
+int rk05_read_disk(uint8_t disk, uint16_t * target){
+ int track,sector;
+ int res;
+ uint16_t status;
+ if (!target){
+ errno=EINVAL;
+ return -1;
+ }
+ res=rk05_recalibrate(disk);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ if (status != RK05_STATUS_OK){
+ err("Status error while recalibrating!\n");
+ ERROR rk05_perror(status);
+ errno=EIO;
+ return -1;
+ }
+
+ for (track=0; track<RK05_TRACKS; track++){
+ info("\rReading drive %i: CYL: %3i, SIDE: %i %s",
+ disk, track/2, track%2,bars(70,RK05_TRACKS-1,track));
+ res=rk05_read_track(disk, track);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+
+ if (status != RK05_STATUS_OK){
+ int retries;
+ info("\nAn error occured!\n");
+ INFO rk05_perror(status);
+ warning("Reading of track %i failed. Will try individual sectors now.\n", track);
+ res=rk05_recalibrate(disk);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ for (sector=0; sector<16; sector++){
+ retries=16;
+ while(retries){
+ debug("Reading sector %i with %i tries left\n",sector,retries);
+ res=rk05_read_sector(disk,track*16+order2[sector],order2[sector]*0400);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ if (status != RK05_STATUS_OK){
+ retries--;
+ warning("Error on sector %i, will retry %i times.\n",sector, retries);
+ INFO rk05_perror(status);
+ res=rk05_recalibrate(disk);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ } else {
+ retries++;
+ break;
+ }
+ }
+ }
+ if(retries==0){
+ err("Failed to read track %i. Giving up.\n",track);
+ errno=EIO;
+ return -1;
+ } else {
+ warning("Now it succeeded.\n");
+ }
+ }
+ res=get_buffer(target+(4096*track));
+ if(res<0) return res;
+ }
+ info("\n");
+ debug("rk05_read_disk():Success\n");
+ return 0;
+}
+
+int rk05_write_disk(uint8_t disk, uint16_t * target){
+ int track,sector;
+ int res;
+ uint16_t status;
+ if (!target){
+ errno=EINVAL;
+ return -1;
+ }
+ res=rk05_recalibrate(disk);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ if (status != RK05_STATUS_OK){
+ err("Status error while recalibrating!\n");
+ ERROR rk05_perror(status);
+ errno=EIO;
+ return -1;
+ }
+ for (track=0; track<RK05_TRACKS; track++){
+ info("\rWriting drive %i: CYL: %3i, SIDE: %i %s", disk, track/2, track%2, bars(70,RK05_TRACKS-1,track));
+ res=put_buffer(target+(4096*track));
+ if (res<0) return res;
+ res=rk05_write_track(disk,track);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+
+ if (status != RK05_STATUS_OK){
+ int retries;
+ info ("\nAn error occured!\n");
+ INFO rk05_perror(status);
+ warning("Writing of track %i failed. Will try individual sectors now.\n", track);
+ res=rk05_recalibrate(disk);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ for (sector=0; sector<16; sector++){
+ retries=16;
+ while(retries){
+ debug("Writing sector %i with %i tries left\n",order2[sector],retries);
+ res=rk05_write_sector(disk,track*16+order2[sector],order2[sector]*0400);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ if (status != RK05_STATUS_OK){
+ retries--;
+ warning("Error on sector %i, will retry %i times.\n",order2[sector], retries);
+ INFO rk05_perror(status);
+ res=rk05_recalibrate(disk);
+ if (res<0) return res;
+ res=rk05_status(&status);
+ if (res<0) return res;
+ } else {
+ retries++;
+ break;
+ }
+ }
+ }
+ if(retries==0){
+ err("Failed to write track %i. Giving up.\n",track);
+ errno=EIO;
+ return -1;
+ } else {
+ warning("Now it succeeded.\n");
+ }
+ }
+ }
+ info("\n");
+ debug("rk05_write_disk():Success\n");
+ return 0;
+}
+
+void rk05_perror(uint16_t status){
+ fprintf(stderr, "Status: %s\n",(status&04000?"Done":"Busy"));
+ if (status & 02000) fprintf(stderr,"Head in motion.\n");
+ if (status & 00400) fprintf(stderr,"Seek fail\n");
+ if (status & 00200) fprintf(stderr,"File not ready\n");
+ if (status & 00100) fprintf(stderr,"Control busy\n");
+ if (status & 00040) fprintf(stderr,"Timing error");
+ if (status & 00020) printf("Write lock error\n");
+ if (status & 00010) fprintf(stderr,"Parity error\n");
+ if (status & 00004) fprintf(stderr,"Data request late\n");
+ if (status & 00002) fprintf(stderr,"Drive status error\n");
+ if (status & 00001) fprintf(stderr,"Cylinder address error\n");
+}