text: //	Copyright (C) 2013 Michael McMaster <michael@codesrc.com>
  //
  //	This file is part of SCSI2SD.
  //
  //	SCSI2SD is free software: you can redistribute it and/or modify
  //	it under the terms of the GNU General Public License as published by
  //	the Free Software Foundation, either version 3 of the License, or
  //	(at your option) any later version.
  //
  //	SCSI2SD is distributed in the hope that it will be useful,
  //	but WITHOUT ANY WARRANTY; without even the implied warranty of
  //	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  //	GNU General Public License for more details.
  //
  //	You should have received a copy of the GNU General Public License
  //	along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
  
  #include "geometry.h"
  #include "scsi.h"
  #include "sd.h"
  #include "config.h"
  
  #include <string.h>
  
  uint32_t getScsiCapacity(
  	uint32_t sdSectorStart,
  	uint16_t bytesPerSector,
  	uint32_t scsiSectors)
  {
  	uint32_t capacity =
  		(sdDev.capacity - sdSectorStart - S2S_CFG_SIZE) /
  			SDSectorsPerSCSISector(bytesPerSector);
  
  
  	if (sdDev.capacity == 0)
  	{
  		capacity = 0;
  	}
  	else if (sdSectorStart >= (sdDev.capacity - S2S_CFG_SIZE))
  	{
  		capacity = 0;
  	}
  	else if (scsiSectors && (capacity > scsiSectors))
  	{
  		capacity = scsiSectors;
  	}
  	return capacity;
  }
  
  
  uint32_t SCSISector2SD(
  	uint32_t sdSectorStart,
  	uint16_t bytesPerSector,
  	uint32_t scsiSector)
  {
  	return scsiSector * SDSectorsPerSCSISector(bytesPerSector) + sdSectorStart;
  }
  
  // Standard mapping according to ECMA-107 and ISO/IEC 9293:1994
  // Sector always starts at 1. There is no 0 sector.
  uint64_t CHS2LBA(
  	uint32_t c,
  	uint8_t h,
  	uint32_t s,
  	uint16_t headsPerCylinder,
  	uint16_t sectorsPerTrack)
  {
  	return (
  		(((uint64_t)c) * headsPerCylinder + h) *
  			(uint64_t) sectorsPerTrack
  		) + (s - 1);
  }
  
  
  void LBA2CHS(
  	uint32_t lba,
  	uint32_t* c,
  	uint8_t* h,
  	uint32_t* s,
  	uint16_t headsPerCylinder,
  	uint16_t sectorsPerTrack)
  {
  	*c = lba / (((uint32_t) sectorsPerTrack) * headsPerCylinder);
  	*h = (lba / sectorsPerTrack) % headsPerCylinder;
  	*s = (lba % sectorsPerTrack) + 1;
  }
  
  uint64_t scsiByteAddress(
  	uint16_t bytesPerSector,
  	uint16_t headsPerCylinder,
  	uint16_t sectorsPerTrack,
  	int format,
  	const uint8_t* addr)
  {
  	uint64_t result;
  	switch (format)
  	{
  	case ADDRESS_BLOCK:
  	{
  		uint32_t lba =
  			(((uint32_t) addr[0]) << 24) +
  			(((uint32_t) addr[1]) << 16) +
  			(((uint32_t) addr[2]) << 8) +
  			addr[3];
  
  		result = (uint64_t) bytesPerSector * lba;
  	} break;
  
  	case ADDRESS_PHYSICAL_BYTE:
  	{
  		uint32_t cyl =
  			(((uint32_t) addr[0]) << 16) +
  			(((uint32_t) addr[1]) << 8) +
  			addr[2];
  
  		uint8_t head = addr[3];
  
  		uint32_t bytes =
  			(((uint32_t) addr[4]) << 24) +
  			(((uint32_t) addr[5]) << 16) +
  			(((uint32_t) addr[6]) << 8) +
  			addr[7];
  
  		result = CHS2LBA(cyl, head, 1, headsPerCylinder, sectorsPerTrack) *
  			(uint64_t) bytesPerSector + bytes;
  	} break;
  
  	case ADDRESS_PHYSICAL_SECTOR:
  	{
  		uint32_t cyl =
  			(((uint32_t) addr[0]) << 16) +
  			(((uint32_t) addr[1]) << 8) +
  			addr[2];
  
  		uint8_t head = scsiDev.data[3];
  
  		uint32_t sector =
  			(((uint32_t) addr[4]) << 24) +
  			(((uint32_t) addr[5]) << 16) +
  			(((uint32_t) addr[6]) << 8) +
  			addr[7];
  
  		result = CHS2LBA(cyl, head, sector, headsPerCylinder, sectorsPerTrack) * (uint64_t) bytesPerSector;
  	} break;
  
  	default:
  		result = (uint64_t) -1;
  	}
  
  	return result;
  }
  
  
  void scsiSaveByteAddress(
  	uint16_t bytesPerSector,
  	uint16_t headsPerCylinder,
  	uint16_t sectorsPerTrack,
  	int format,
  	uint64_t byteAddr,
  	uint8_t* buf)
  {
  	uint32_t lba = byteAddr / bytesPerSector;
  	uint32_t byteOffset = byteAddr % bytesPerSector;
  
  	switch (format)
  	{
  	case ADDRESS_BLOCK:
  	{
  		buf[0] = lba >> 24;
  		buf[1] = lba >> 16;
  		buf[2] = lba >> 8;
  		buf[3] = lba;
  
  		buf[4] = 0;
  		buf[5] = 0;
  		buf[6] = 0;
  		buf[7] = 0;
  	} break;
  
  	case ADDRESS_PHYSICAL_BYTE:
  	{
  		uint32_t cyl;
  		uint8_t head;
  		uint32_t sector;
  		uint32_t bytes;
  
  		LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);
  
  		bytes = sector * bytesPerSector + byteOffset;
  
  		buf[0] = cyl >> 16;
  		buf[1] = cyl >> 8;
  		buf[2] = cyl;
  
  		buf[3] = head;
  
  		buf[4] = bytes >> 24;
  		buf[5] = bytes >> 16;
  		buf[6] = bytes >> 8;
  		buf[7] = bytes;
  	} break;
  
  	case ADDRESS_PHYSICAL_SECTOR:
  	{
  		uint32_t cyl;
  		uint8_t head;
  		uint32_t sector;
  
  		LBA2CHS(lba, &cyl, &head, &sector, headsPerCylinder, sectorsPerTrack);
  
  		buf[0] = cyl >> 16;
  		buf[1] = cyl >> 8;
  		buf[2] = cyl;
  
  		buf[3] = head;
  
  		buf[4] = sector >> 24;
  		buf[5] = sector >> 16;
  		buf[6] = sector >> 8;
  		buf[7] = sector;
  	} break;
  
  	default:
  		memset(buf, 0, 8);
  	}
  
  }
  
  