Quelltext /~heha/basteln/PC/Programmiergeräte/Wild GIF12/mtp1.zip/MTP.cpp

// MTP.h - MTP Responder

#include "MTP.h"
#include "myFlash.h"
#include "usb.h"
#include <string.h>

struct MTPHeader {
 union{
  byte lenL;  // 0
  word lenW;
  uint32_t lenD;
  byte lenB[4];
 };
 byte typeL,typeH; // 4
 byte opL,opH;   // 6
 uint32_t transid; // 8
};

struct{
 MTPHeader h;
 uint32_t params[3];    // 12
}bulkcs;

struct event_t{
 byte codeL;
 byte codeH;
 uint32_t sessid;
 uint32_t transid;
 uint32_t params[3];
 void send(byte,byte);
}event;

void event_t::send(byte code,byte param) {
 codeL=code;
 codeH=0x40;
 *(byte*)params = param;
 usbSend(this,sizeof*this,3);
 usbFlush();
}

static byte utf16len(const char*str) {	// Länge für UTF-16 in Words, str!=nullptr
 byte r=0;
 while (takeUTF8(str)) ++r;
 return r;
}

namespace MTP {
char extendedeventdata[8];
devicestatus_t devicestatus={4,0x2001};

static void outc(const byte*p,byte len) {
 usbSend(p,len);
}
static void out1(byte b) {
 outc(&b,1);
}
static void out2(word w) {
 out1(w);
 out1(w>>8);
}
static void out4(word w) {
 out2(w);
 out2(0);
}
static void out0(byte len) {
 usbSend(0,len);
}
static void out8(word w) {
 out2(w);
 out0(6);
}
static void outHeader(word len) {
 out4(len+12);
 out2(2);		// type
 outc(&bulkcs.h.opL,6);	// operation + transaction ID
}
static void outFlush() {
 usbFlush();
}
static void outS(const char*s) {
 byte len=utf16len(s)+1;
 out1(len);
 do out2(takeUTF8(s)); while(--len);
}
static byte lenS(const char*s) {	// Bytes für String
 return 1+(utf16len(s)+1<<1);
}

static void outDescriptor() {
 static const PROGMEM char Model[]="GIF reader";
 outHeader(11+10+17+lenS(FP(Model))+2);
 out0(11);
 out4(3);	// Only needed capabilities (Windows 10)
 out2(0x100B);  // DeleteObject		// required otherwise files are deletion protected
 out2(0x100C);  // SendObjectInfo	// required otherwise files cannot be added
 out2(0x100D);  // SendObject		// same as above
 out0(17);		// lot of garbage
 outS(FP(Model));	// sonst erscheint „Tragbares Gerät“
 out0(2);
 outFlush();
}

static void outStorageIDs() {
 outHeader(8);
 out4(1); // 1 entry
 out4(1); // 1 storage
 outFlush();
}

static void outStorageInfo() {
 static const PROGMEM char Medium[]="GRM10 card";
 outHeader(2+2+2+8+8+4+lenS(FP(Medium))+1);
 out2(3);	// storage type (fixed RAM)
 out2(1);	// filesystem type (generic flat)
 out2(0);	// access capability (read-write)
 out8(myFlash::getCapacity());	// max capacity (bytes)
 out8(myFlash::getFreeSpace());	// free space (bytes)
 out4(myFlash::getFreeObjects());	// free space (objects)
 outS(FP(Medium)); // storage descriptor,	sonst erscheint „Wechselmedien“
 out0(1);	// volume identifier
 outFlush();
}

static void outObjectHandles() {
 byte num = myFlash::GetNumObjects();
 outHeader(4+4*num);
 out4(num);
 for (byte a=1; a<myFlash::FSTART; a++) {
  if (myFlash::eer(a)!=0xFF) out4(a);
 }
 outFlush();
}

static void outObjectInfo(byte handle) {
 const myFlash::sec_t*s=myFlash::GetObjectInfo(handle);
 outHeader(4+4+4+40+lenS(s->dirent.name)+33+33+1);
 out4(1); // Logical Storage ID
 out4(0x3000);	// undefiniert, Text
 out4(pgm_read_word(&s->dirent.sizeL)); // size
 out0(40); // Info für Bilder
 outS(s->dirent.name);
 outS(s->dirent.ctime);  // date captured - automatischer Typecast-Operator!
 outS(s->dirent.mtime);  // date modified - automatischer Typecast-Operator!
 out0(1);  // keywords
 outFlush();
}

static bool GetObject(byte handle) {
 byte b=myFlash::eer(handle);	// 1. Sektor (muss gültig sein)
 if (b<myFlash::FSTART || b>=myFlash::FEND1) return false;
 const char*addr=FP((const char*)(b<<myFlash::SEC_SH));
 unsigned len=pgm_read_word(addr);	// Datei-Länge (sollte der Sektor-Kette entsprechen!)
 outHeader(len);
 while (len) {
  b=myFlash::eer(b);		// (Nächster) Datensektor
  if (b<myFlash::FSTART || b>=myFlash::FEND1) return false;	// Fehler!
  addr=FP((const char*)(b<<myFlash::SEC_SH));
  unsigned l=1<<myFlash::SEC_SH; if (l>len) l=len;
  outc((const byte*)addr,l);	// direkt aus Flash
  len-=l;
 }
 outFlush();	// Short-Packet
 return true;
}

static myFlash::sec_t secbuf  __attribute__((section(".noinit")));

extern "C" void setutf8(word utf16/*,char*&Z*/) __attribute__((naked));
void setutf8(word utf16) {asm(
"	cpi	r24,0x80	\n"
"	cpc	r25,r1		\n"
"	brcs	1f		\n"	// 1-Byte-UTF8
"	movw	r18,r24		\n"
"	lsl	r18		\n"
"	rol	r25		\n"
"	lsl	r18		\n"
"	rol	r25		\n"
"	andi	r24,0x3F	\n"
"	ori	r24,0x80	\n"
"	ori	r25,0xC0	\n"
"	cpi	r19,0x08	\n"
"	brcs	2f		\n"	// 2-Byte-UTF8
"	andi	r25,0xBF	\n"
"	swap	r19		\n"
"	andi	r19,0x0F	\n"
"	ori	r19,0xE0	\n"
"	st	Z+,r19		\n"
"2:	st	Z+,r25		\n"
"1:	st	Z+,r24		\n"
"	ret			\n"
);}
// Zeiger rückt um 1..3 Byte vor
#if 0
#define dropUTF8(c,s) (__extension__({	\
 asm("\tmovw r24,%1\n\trcall setutf8\n":"+z"(s):"r"(c):"r18","r19","r24","r25");\
}))
#else
static void dropUTF8(word c,char*&s) {
asm("	movw	r24,%1	\n"
"	ld	ZL,X+	\n"
"	ld	ZH,X+	\n"
"	rcall	setutf8	\n"
"	st	-X,ZH	\n"
"	st	-X,ZL	\n"
::"x"(&s),"r"(c):"r18","r19","r24","r25");}
#endif

static byte usbGetString(char*s,char space) {
 byte n;
 usbRecv(&n,1);
 if (n) for (byte i=0; i<n; i++) {
  word w;
  usbRecv(&w,2);	// UTF-16-Zeichen + Nullterminierung
  if (space>0) {
   space--;
   if (w>=0x80) space--;
   if (w>=0x2000) space--;
   if (space<=0) *s=0;	// nullterminieren wenn Puffer kleiner
   else dropUTF8(w,s);
  }
 }
 else if (space) *s=0;	// immer nullterminieren
 return 1+n+n;		// Anzahl von USB abgeholte Bytes
}

// Datei erzeugen: Hier: Verzeichniseintrag initialisieren
static bool SendObjectInfo() {
 usbRecv(0,20);	// Header, Dateityp ignorieren
 secbuf.empty();	// mit 0xFF füllen
 usbRecv(&secbuf.dirent.size,4);
 usbRecv(0,40);	// Krimskrams dazwischen ignorieren
 usbGetString(secbuf.dirent.name,sizeof secbuf.dirent.name);
 usbGetString(fattime::string,sizeof fattime::string);
 secbuf.dirent.ctime();
 usbGetString(fattime::string,sizeof fattime::string);
 secbuf.dirent.mtime();
 usbGetString(nullptr,0);	// Rest („keywords“) auslesen
 return myFlash::Create(&secbuf);
}

// Datei erzeugen: Jetzt kommen die Daten, und Dateilänge sollte gleich sein.
static bool SendObject() {
 word size;
 usbRecv(&size,2);
 usbRecv(0,10);
 size-=12;
 while (size) {
  byte l=size>sizeof secbuf.data?sizeof secbuf.data:size;
  secbuf.empty();
  usbRecv(secbuf.data,l);
  if (!myFlash::writeNextPage(&secbuf)) return false;
  size-=l;
 }
 return true;
}

static void handleCommand() {
 byte p1 = 0;
 byte retcode = 1;  // Ok
 event.transid=bulkcs.h.transid;
 if (bulkcs.h.opH!=0x10) retcode=5;
 else switch (bulkcs.h.opL) {
  case 1: outDescriptor();		break;	// GetDescription
  case 2: event.sessid=bulkcs.params[0];break;	// OpenSession
  case 3: event.sessid=0;		break;	// CloseSession
  case 4: outStorageIDs();		break;	// GetStorageIDs
  case 5: outStorageInfo();		break;	// GetStorageInfo
  case 6: p1 = myFlash::GetNumObjects();break;	// GetNumObjects
  case 7: outObjectHandles();		break;	// GetObjectHandles
  case 8: outObjectInfo(bulkcs.params[0]);break;// GetObjectInfo
  case 9: if (!GetObject(bulkcs.params[0])) retcode=21; break;	// GetObject
  case 11:  // DeleteObject
  if (!myFlash::DeleteObject(bulkcs.params[0])) retcode = 0x12; // partial deletion
  else event.send(3/*object removed*/,bulkcs.params[0]);
  break;
  case 12:  // SendObjectInfo
  if (!SendObjectInfo()) retcode=12;	// store_full
  else bulkcs.params[2] = myFlash::createdDirEnt;
  bulkcs.h.lenL = 12 + 3*4;	// 3 Parameter
  goto ex1;
  case 13:	// SendObject
  if (!SendObject()) retcode=21;	// no_valid_objectinfo
  bulkcs.h.lenL = 12;
  event.send(2/*object added*/,myFlash::createdDirEnt);
  break;
  default:
  retcode = 5;  // operation not supported
 }
 bulkcs.h.lenL = 16;	// Standard: Rückgabe 1 Parameter 
 bulkcs.params[0] = p1;
ex1:			// für mehr als 1 Parameter
 bulkcs.h.typeL = 3;	// Antwort
 bulkcs.h.opL = retcode;
 bulkcs.h.opH = 0x20;
 usbSend(&bulkcs,bulkcs.h.lenL);
 usbFlush();
}

// Einzige exportierte Funktion
void pollUSB() {
 if (usbRecvPoll()) {
  PORTD&=~0x20;	// TX
  usbRecv(&bulkcs,8);
  if (bulkcs.h.typeL==1	// "command", alles andere: STALL (Protokollfehler)
//  && bulkcs.h.typeH==0x10
  && bulkcs.h.lenD<=sizeof bulkcs
  && bulkcs.h.lenL>=12) {
   usbRecv(&bulkcs.h.transid,bulkcs.h.lenL-8);	// Rest des Kommandos abholen
   handleCommand();
  }else{
//   eelog(&bulkcs,8);	// Murks!
   UEINTX=0;		// aufs nächste ganze USB-Paket synchronisieren
  }
  PORTD|= 0x20;
 }
}

}/*namespace*/
Vorgefundene Kodierung: UTF-80