mxsimage.c 50 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365
  1. // SPDX-License-Identifier: GPL-2.0+
  2. /*
  3. * Freescale i.MX23/i.MX28 SB image generator
  4. *
  5. * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de>
  6. */
  7. #ifdef CONFIG_MXS
  8. #include <errno.h>
  9. #include <fcntl.h>
  10. #include <stdio.h>
  11. #include <string.h>
  12. #include <unistd.h>
  13. #include <limits.h>
  14. #include <openssl/evp.h>
  15. #include "imagetool.h"
  16. #include "mxsimage.h"
  17. #include "pbl_crc32.h"
  18. #include <image.h>
  19. /*
  20. * OpenSSL 1.1.0 and newer compatibility functions:
  21. * https://wiki.openssl.org/index.php/1.1_API_Changes
  22. */
  23. #if OPENSSL_VERSION_NUMBER < 0x10100000L || \
  24. (defined(LIBRESSL_VERSION_NUMBER) && LIBRESSL_VERSION_NUMBER < 0x2070000fL)
  25. static void *OPENSSL_zalloc(size_t num)
  26. {
  27. void *ret = OPENSSL_malloc(num);
  28. if (ret != NULL)
  29. memset(ret, 0, num);
  30. return ret;
  31. }
  32. EVP_MD_CTX *EVP_MD_CTX_new(void)
  33. {
  34. return OPENSSL_zalloc(sizeof(EVP_MD_CTX));
  35. }
  36. void EVP_MD_CTX_free(EVP_MD_CTX *ctx)
  37. {
  38. EVP_MD_CTX_cleanup(ctx);
  39. OPENSSL_free(ctx);
  40. }
  41. int EVP_CIPHER_CTX_reset(EVP_CIPHER_CTX *ctx)
  42. {
  43. return EVP_CIPHER_CTX_cleanup(ctx);
  44. }
  45. #endif
  46. /*
  47. * DCD block
  48. * |-Write to address command block
  49. * | 0xf00 == 0xf33d
  50. * | 0xba2 == 0xb33f
  51. * |-ORR address with mask command block
  52. * | 0xf00 |= 0x1337
  53. * |-Write to address command block
  54. * | 0xba2 == 0xd00d
  55. * :
  56. */
  57. #define SB_HAB_DCD_WRITE 0xccUL
  58. #define SB_HAB_DCD_CHECK 0xcfUL
  59. #define SB_HAB_DCD_NOOP 0xc0UL
  60. #define SB_HAB_DCD_MASK_BIT (1 << 3)
  61. #define SB_HAB_DCD_SET_BIT (1 << 4)
  62. /* Addr.n = Value.n */
  63. #define SB_DCD_WRITE \
  64. (SB_HAB_DCD_WRITE << 24)
  65. /* Addr.n &= ~Value.n */
  66. #define SB_DCD_ANDC \
  67. ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT)
  68. /* Addr.n |= Value.n */
  69. #define SB_DCD_ORR \
  70. ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  71. /* (Addr.n & Value.n) == 0 */
  72. #define SB_DCD_CHK_EQZ \
  73. (SB_HAB_DCD_CHECK << 24)
  74. /* (Addr.n & Value.n) == Value.n */
  75. #define SB_DCD_CHK_EQ \
  76. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT)
  77. /* (Addr.n & Value.n) != Value.n */
  78. #define SB_DCD_CHK_NEQ \
  79. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT)
  80. /* (Addr.n & Value.n) != 0 */
  81. #define SB_DCD_CHK_NEZ \
  82. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  83. /* NOP */
  84. #define SB_DCD_NOOP \
  85. (SB_HAB_DCD_NOOP << 24)
  86. struct sb_dcd_ctx {
  87. struct sb_dcd_ctx *dcd;
  88. uint32_t id;
  89. /* The DCD block. */
  90. uint32_t *payload;
  91. /* Size of the whole DCD block. */
  92. uint32_t size;
  93. /* Pointer to previous DCD command block. */
  94. uint32_t *prev_dcd_head;
  95. };
  96. /*
  97. * IMAGE
  98. * |-SECTION
  99. * | |-CMD
  100. * | |-CMD
  101. * | `-CMD
  102. * |-SECTION
  103. * | |-CMD
  104. * : :
  105. */
  106. struct sb_cmd_list {
  107. char *cmd;
  108. size_t len;
  109. unsigned int lineno;
  110. };
  111. struct sb_cmd_ctx {
  112. uint32_t size;
  113. struct sb_cmd_ctx *cmd;
  114. uint8_t *data;
  115. uint32_t length;
  116. struct sb_command payload;
  117. struct sb_command c_payload;
  118. };
  119. struct sb_section_ctx {
  120. uint32_t size;
  121. /* Section flags */
  122. unsigned int boot:1;
  123. struct sb_section_ctx *sect;
  124. struct sb_cmd_ctx *cmd_head;
  125. struct sb_cmd_ctx *cmd_tail;
  126. struct sb_sections_header payload;
  127. };
  128. struct sb_image_ctx {
  129. unsigned int in_section:1;
  130. unsigned int in_dcd:1;
  131. /* Image configuration */
  132. unsigned int display_progress:1;
  133. unsigned int silent_dump:1;
  134. char *input_filename;
  135. char *output_filename;
  136. char *cfg_filename;
  137. uint8_t image_key[16];
  138. /* Number of section in the image */
  139. unsigned int sect_count;
  140. /* Bootable section */
  141. unsigned int sect_boot;
  142. unsigned int sect_boot_found:1;
  143. struct sb_section_ctx *sect_head;
  144. struct sb_section_ctx *sect_tail;
  145. struct sb_dcd_ctx *dcd_head;
  146. struct sb_dcd_ctx *dcd_tail;
  147. EVP_CIPHER_CTX *cipher_ctx;
  148. EVP_MD_CTX *md_ctx;
  149. uint8_t digest[32];
  150. struct sb_key_dictionary_key sb_dict_key;
  151. struct sb_boot_image_header payload;
  152. };
  153. /*
  154. * Instruction semantics:
  155. * NOOP
  156. * TAG [LAST]
  157. * LOAD address file
  158. * LOAD IVT address IVT_entry_point
  159. * FILL address pattern length
  160. * JUMP [HAB] address [r0_arg]
  161. * CALL [HAB] address [r0_arg]
  162. * MODE mode
  163. * For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
  164. * JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
  165. * For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
  166. * JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
  167. */
  168. /*
  169. * AES libcrypto
  170. */
  171. static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc)
  172. {
  173. EVP_CIPHER_CTX *ctx;
  174. int ret;
  175. /* If there is no init vector, init vector is all zeroes. */
  176. if (!iv)
  177. iv = ictx->image_key;
  178. ctx = EVP_CIPHER_CTX_new();
  179. ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc);
  180. if (ret == 1) {
  181. EVP_CIPHER_CTX_set_padding(ctx, 0);
  182. ictx->cipher_ctx = ctx;
  183. }
  184. return ret;
  185. }
  186. static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data,
  187. uint8_t *out_data, int in_len)
  188. {
  189. EVP_CIPHER_CTX *ctx = ictx->cipher_ctx;
  190. int ret, outlen;
  191. uint8_t *outbuf;
  192. outbuf = malloc(in_len);
  193. if (!outbuf)
  194. return -ENOMEM;
  195. memset(outbuf, 0, sizeof(in_len));
  196. ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len);
  197. if (!ret) {
  198. ret = -EINVAL;
  199. goto err;
  200. }
  201. if (out_data)
  202. memcpy(out_data, outbuf, outlen);
  203. err:
  204. free(outbuf);
  205. return ret;
  206. }
  207. static int sb_aes_deinit(EVP_CIPHER_CTX *ctx)
  208. {
  209. return EVP_CIPHER_CTX_reset(ctx);
  210. }
  211. static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc)
  212. {
  213. int ret;
  214. EVP_CIPHER_CTX *ctx = ictx->cipher_ctx;
  215. struct sb_boot_image_header *sb_header = &ictx->payload;
  216. uint8_t *iv = sb_header->iv;
  217. ret = sb_aes_deinit(ctx);
  218. if (!ret)
  219. return ret;
  220. return sb_aes_init(ictx, iv, enc);
  221. }
  222. /*
  223. * Debug
  224. */
  225. static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...)
  226. {
  227. va_list ap;
  228. if (ictx->silent_dump)
  229. return;
  230. va_start(ap, fmt);
  231. vfprintf(stdout, fmt, ap);
  232. va_end(ap);
  233. }
  234. /*
  235. * Code
  236. */
  237. static time_t sb_get_timestamp(void)
  238. {
  239. struct tm time_2000 = {
  240. .tm_yday = 1, /* Jan. 1st */
  241. .tm_year = 100, /* 2000 */
  242. };
  243. time_t seconds_to_2000 = mktime(&time_2000);
  244. time_t seconds_to_now = time(NULL);
  245. return seconds_to_now - seconds_to_2000;
  246. }
  247. static int sb_get_time(time_t time, struct tm *tm)
  248. {
  249. struct tm time_2000 = {
  250. .tm_yday = 1, /* Jan. 1st */
  251. .tm_year = 0, /* 1900 */
  252. };
  253. const time_t seconds_to_2000 = mktime(&time_2000);
  254. const time_t seconds_to_now = seconds_to_2000 + time;
  255. struct tm *ret;
  256. ret = gmtime_r(&seconds_to_now, tm);
  257. return ret ? 0 : -EINVAL;
  258. }
  259. static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
  260. {
  261. EVP_MD_CTX *md_ctx = ictx->md_ctx;
  262. struct sb_boot_image_header *sb_header = &ictx->payload;
  263. uint8_t *sb_header_ptr = (uint8_t *)sb_header;
  264. /* Encrypt the header, compute the digest. */
  265. sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
  266. EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
  267. }
  268. static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx)
  269. {
  270. EVP_MD_CTX *md_ctx = ictx->md_ctx;
  271. struct sb_section_ctx *sctx = ictx->sect_head;
  272. struct sb_sections_header *shdr;
  273. uint8_t *sb_sections_header_ptr;
  274. const int size = sizeof(*shdr);
  275. while (sctx) {
  276. shdr = &sctx->payload;
  277. sb_sections_header_ptr = (uint8_t *)shdr;
  278. sb_aes_crypt(ictx, sb_sections_header_ptr,
  279. ictx->sb_dict_key.cbc_mac, size);
  280. EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);
  281. sctx = sctx->sect;
  282. };
  283. }
  284. static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx)
  285. {
  286. EVP_MD_CTX *md_ctx = ictx->md_ctx;
  287. sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key,
  288. sizeof(ictx->sb_dict_key.key));
  289. EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
  290. }
  291. static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx)
  292. {
  293. EVP_MD_CTX *md_ctx = ictx->md_ctx;
  294. EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
  295. sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key,
  296. sizeof(ictx->sb_dict_key.key));
  297. }
  298. static void sb_encrypt_tag(struct sb_image_ctx *ictx,
  299. struct sb_cmd_ctx *cctx)
  300. {
  301. EVP_MD_CTX *md_ctx = ictx->md_ctx;
  302. struct sb_command *cmd = &cctx->payload;
  303. sb_aes_crypt(ictx, (uint8_t *)cmd,
  304. (uint8_t *)&cctx->c_payload, sizeof(*cmd));
  305. EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
  306. }
  307. static int sb_encrypt_image(struct sb_image_ctx *ictx)
  308. {
  309. /* Start image-wide crypto. */
  310. ictx->md_ctx = EVP_MD_CTX_new();
  311. EVP_DigestInit(ictx->md_ctx, EVP_sha1());
  312. /*
  313. * SB image header.
  314. */
  315. sb_aes_init(ictx, NULL, 1);
  316. sb_encrypt_sb_header(ictx);
  317. /*
  318. * SB sections header.
  319. */
  320. sb_encrypt_sb_sections_header(ictx);
  321. /*
  322. * Key dictionary.
  323. */
  324. sb_aes_reinit(ictx, 1);
  325. sb_encrypt_key_dictionary_key(ictx);
  326. /*
  327. * Section tags.
  328. */
  329. struct sb_cmd_ctx *cctx;
  330. struct sb_command *ccmd;
  331. struct sb_section_ctx *sctx = ictx->sect_head;
  332. while (sctx) {
  333. cctx = sctx->cmd_head;
  334. sb_aes_reinit(ictx, 1);
  335. while (cctx) {
  336. ccmd = &cctx->payload;
  337. sb_encrypt_tag(ictx, cctx);
  338. if (ccmd->header.tag == ROM_TAG_CMD) {
  339. sb_aes_reinit(ictx, 1);
  340. } else if (ccmd->header.tag == ROM_LOAD_CMD) {
  341. sb_aes_crypt(ictx, cctx->data, cctx->data,
  342. cctx->length);
  343. EVP_DigestUpdate(ictx->md_ctx, cctx->data,
  344. cctx->length);
  345. }
  346. cctx = cctx->cmd;
  347. }
  348. sctx = sctx->sect;
  349. };
  350. /*
  351. * Dump the SHA1 of the whole image.
  352. */
  353. sb_aes_reinit(ictx, 1);
  354. EVP_DigestFinal(ictx->md_ctx, ictx->digest, NULL);
  355. EVP_MD_CTX_free(ictx->md_ctx);
  356. sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));
  357. /* Stop the encryption session. */
  358. sb_aes_deinit(ictx->cipher_ctx);
  359. return 0;
  360. }
  361. static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
  362. {
  363. long real_size, roundup_size;
  364. uint8_t *data;
  365. long ret;
  366. unsigned long size;
  367. FILE *fp;
  368. if (!filename) {
  369. fprintf(stderr, "ERR: Missing filename!\n");
  370. return -EINVAL;
  371. }
  372. fp = fopen(filename, "r");
  373. if (!fp)
  374. goto err_open;
  375. ret = fseek(fp, 0, SEEK_END);
  376. if (ret < 0)
  377. goto err_file;
  378. real_size = ftell(fp);
  379. if (real_size < 0)
  380. goto err_file;
  381. ret = fseek(fp, 0, SEEK_SET);
  382. if (ret < 0)
  383. goto err_file;
  384. roundup_size = roundup(real_size, SB_BLOCK_SIZE);
  385. data = calloc(1, roundup_size);
  386. if (!data)
  387. goto err_file;
  388. size = fread(data, 1, real_size, fp);
  389. if (size != (unsigned long)real_size)
  390. goto err_alloc;
  391. cctx->data = data;
  392. cctx->length = roundup_size;
  393. fclose(fp);
  394. return 0;
  395. err_alloc:
  396. free(data);
  397. err_file:
  398. fclose(fp);
  399. err_open:
  400. fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
  401. return -EINVAL;
  402. }
  403. static uint8_t sb_command_checksum(struct sb_command *inst)
  404. {
  405. uint8_t *inst_ptr = (uint8_t *)inst;
  406. uint8_t csum = 0;
  407. unsigned int i;
  408. for (i = 0; i < sizeof(struct sb_command); i++)
  409. csum += inst_ptr[i];
  410. return csum;
  411. }
  412. static int sb_token_to_long(char *tok, uint32_t *rid)
  413. {
  414. char *endptr;
  415. unsigned long id;
  416. if (tok[0] != '0' || tok[1] != 'x') {
  417. fprintf(stderr, "ERR: Invalid hexadecimal number!\n");
  418. return -EINVAL;
  419. }
  420. tok += 2;
  421. errno = 0;
  422. id = strtoul(tok, &endptr, 16);
  423. if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) {
  424. fprintf(stderr, "ERR: Value can't be decoded!\n");
  425. return -EINVAL;
  426. }
  427. /* Check for 32-bit overflow. */
  428. if (id > 0xffffffff) {
  429. fprintf(stderr, "ERR: Value too big!\n");
  430. return -EINVAL;
  431. }
  432. if (endptr == tok) {
  433. fprintf(stderr, "ERR: Deformed value!\n");
  434. return -EINVAL;
  435. }
  436. *rid = (uint32_t)id;
  437. return 0;
  438. }
  439. static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size)
  440. {
  441. uint32_t *tmp;
  442. if (!inc_size)
  443. return 0;
  444. dctx->size += inc_size;
  445. tmp = realloc(dctx->payload, dctx->size);
  446. if (!tmp)
  447. return -ENOMEM;
  448. dctx->payload = tmp;
  449. /* Assemble and update the HAB DCD header. */
  450. dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) |
  451. (dctx->size << 8) |
  452. SB_HAB_VERSION);
  453. return 0;
  454. }
  455. static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  456. {
  457. struct sb_dcd_ctx *dctx;
  458. char *tok;
  459. uint32_t id;
  460. int ret;
  461. dctx = calloc(1, sizeof(*dctx));
  462. if (!dctx)
  463. return -ENOMEM;
  464. ret = sb_grow_dcd(dctx, 4);
  465. if (ret)
  466. goto err_dcd;
  467. /* Read DCD block number. */
  468. tok = strtok(cmd->cmd, " ");
  469. if (!tok) {
  470. fprintf(stderr, "#%i ERR: DCD block without number!\n",
  471. cmd->lineno);
  472. ret = -EINVAL;
  473. goto err_dcd;
  474. }
  475. /* Parse the DCD block number. */
  476. ret = sb_token_to_long(tok, &id);
  477. if (ret) {
  478. fprintf(stderr, "#%i ERR: Malformed DCD block number!\n",
  479. cmd->lineno);
  480. goto err_dcd;
  481. }
  482. dctx->id = id;
  483. /*
  484. * The DCD block is now constructed. Append it to the list.
  485. * WARNING: The DCD size is still not computed and will be
  486. * updated while parsing it's commands.
  487. */
  488. if (!ictx->dcd_head) {
  489. ictx->dcd_head = dctx;
  490. ictx->dcd_tail = dctx;
  491. } else {
  492. ictx->dcd_tail->dcd = dctx;
  493. ictx->dcd_tail = dctx;
  494. }
  495. return 0;
  496. err_dcd:
  497. free(dctx->payload);
  498. free(dctx);
  499. return ret;
  500. }
  501. static int sb_build_dcd_block(struct sb_image_ctx *ictx,
  502. struct sb_cmd_list *cmd,
  503. uint32_t type)
  504. {
  505. char *tok;
  506. uint32_t address, value, length;
  507. int ret;
  508. struct sb_dcd_ctx *dctx = ictx->dcd_tail;
  509. uint32_t *dcd;
  510. if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) &&
  511. ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) {
  512. /* Same instruction as before, just append it. */
  513. ret = sb_grow_dcd(dctx, 8);
  514. if (ret)
  515. return ret;
  516. } else if (type == SB_DCD_NOOP) {
  517. ret = sb_grow_dcd(dctx, 4);
  518. if (ret)
  519. return ret;
  520. /* Update DCD command block pointer. */
  521. dctx->prev_dcd_head = dctx->payload +
  522. dctx->size / sizeof(*dctx->payload) - 1;
  523. /* NOOP has only 4 bytes and no payload. */
  524. goto noop;
  525. } else {
  526. /*
  527. * Either a different instruction block started now
  528. * or this is the first instruction block.
  529. */
  530. ret = sb_grow_dcd(dctx, 12);
  531. if (ret)
  532. return ret;
  533. /* Update DCD command block pointer. */
  534. dctx->prev_dcd_head = dctx->payload +
  535. dctx->size / sizeof(*dctx->payload) - 3;
  536. }
  537. dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2;
  538. /*
  539. * Prepare the command.
  540. */
  541. tok = strtok(cmd->cmd, " ");
  542. if (!tok) {
  543. fprintf(stderr, "#%i ERR: Missing DCD address!\n",
  544. cmd->lineno);
  545. ret = -EINVAL;
  546. goto err;
  547. }
  548. /* Read DCD destination address. */
  549. ret = sb_token_to_long(tok, &address);
  550. if (ret) {
  551. fprintf(stderr, "#%i ERR: Incorrect DCD address!\n",
  552. cmd->lineno);
  553. goto err;
  554. }
  555. tok = strtok(NULL, " ");
  556. if (!tok) {
  557. fprintf(stderr, "#%i ERR: Missing DCD value!\n",
  558. cmd->lineno);
  559. ret = -EINVAL;
  560. goto err;
  561. }
  562. /* Read DCD operation value. */
  563. ret = sb_token_to_long(tok, &value);
  564. if (ret) {
  565. fprintf(stderr, "#%i ERR: Incorrect DCD value!\n",
  566. cmd->lineno);
  567. goto err;
  568. }
  569. /* Fill in the new DCD entry. */
  570. dcd[0] = htonl(address);
  571. dcd[1] = htonl(value);
  572. noop:
  573. /* Update the DCD command block. */
  574. length = dctx->size -
  575. ((dctx->prev_dcd_head - dctx->payload) *
  576. sizeof(*dctx->payload));
  577. dctx->prev_dcd_head[0] = htonl(type | (length << 8));
  578. err:
  579. return ret;
  580. }
  581. static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  582. {
  583. struct sb_section_ctx *sctx;
  584. struct sb_sections_header *shdr;
  585. char *tok;
  586. uint32_t bootable = 0;
  587. uint32_t id;
  588. int ret;
  589. sctx = calloc(1, sizeof(*sctx));
  590. if (!sctx)
  591. return -ENOMEM;
  592. /* Read section number. */
  593. tok = strtok(cmd->cmd, " ");
  594. if (!tok) {
  595. fprintf(stderr, "#%i ERR: Section without number!\n",
  596. cmd->lineno);
  597. ret = -EINVAL;
  598. goto err_sect;
  599. }
  600. /* Parse the section number. */
  601. ret = sb_token_to_long(tok, &id);
  602. if (ret) {
  603. fprintf(stderr, "#%i ERR: Malformed section number!\n",
  604. cmd->lineno);
  605. goto err_sect;
  606. }
  607. /* Read section's BOOTABLE flag. */
  608. tok = strtok(NULL, " ");
  609. if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8))
  610. bootable = SB_SECTION_FLAG_BOOTABLE;
  611. sctx->boot = bootable;
  612. shdr = &sctx->payload;
  613. shdr->section_number = id;
  614. shdr->section_flags = bootable;
  615. /*
  616. * The section is now constructed. Append it to the list.
  617. * WARNING: The section size is still not computed and will
  618. * be updated while parsing it's commands.
  619. */
  620. ictx->sect_count++;
  621. /* Mark that this section is bootable one. */
  622. if (bootable) {
  623. if (ictx->sect_boot_found) {
  624. fprintf(stderr,
  625. "#%i WARN: Multiple bootable section!\n",
  626. cmd->lineno);
  627. } else {
  628. ictx->sect_boot = id;
  629. ictx->sect_boot_found = 1;
  630. }
  631. }
  632. if (!ictx->sect_head) {
  633. ictx->sect_head = sctx;
  634. ictx->sect_tail = sctx;
  635. } else {
  636. ictx->sect_tail->sect = sctx;
  637. ictx->sect_tail = sctx;
  638. }
  639. return 0;
  640. err_sect:
  641. free(sctx);
  642. return ret;
  643. }
  644. static int sb_build_command_nop(struct sb_image_ctx *ictx)
  645. {
  646. struct sb_section_ctx *sctx = ictx->sect_tail;
  647. struct sb_cmd_ctx *cctx;
  648. struct sb_command *ccmd;
  649. cctx = calloc(1, sizeof(*cctx));
  650. if (!cctx)
  651. return -ENOMEM;
  652. ccmd = &cctx->payload;
  653. /*
  654. * Construct the command.
  655. */
  656. ccmd->header.checksum = 0x5a;
  657. ccmd->header.tag = ROM_NOP_CMD;
  658. cctx->size = sizeof(*ccmd);
  659. /*
  660. * Append the command to the last section.
  661. */
  662. if (!sctx->cmd_head) {
  663. sctx->cmd_head = cctx;
  664. sctx->cmd_tail = cctx;
  665. } else {
  666. sctx->cmd_tail->cmd = cctx;
  667. sctx->cmd_tail = cctx;
  668. }
  669. return 0;
  670. }
  671. static int sb_build_command_tag(struct sb_image_ctx *ictx,
  672. struct sb_cmd_list *cmd)
  673. {
  674. struct sb_section_ctx *sctx = ictx->sect_tail;
  675. struct sb_cmd_ctx *cctx;
  676. struct sb_command *ccmd;
  677. char *tok;
  678. cctx = calloc(1, sizeof(*cctx));
  679. if (!cctx)
  680. return -ENOMEM;
  681. ccmd = &cctx->payload;
  682. /*
  683. * Prepare the command.
  684. */
  685. /* Check for the LAST keyword. */
  686. tok = strtok(cmd->cmd, " ");
  687. if (tok && !strcmp(tok, "LAST"))
  688. ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG;
  689. /*
  690. * Construct the command.
  691. */
  692. ccmd->header.checksum = 0x5a;
  693. ccmd->header.tag = ROM_TAG_CMD;
  694. cctx->size = sizeof(*ccmd);
  695. /*
  696. * Append the command to the last section.
  697. */
  698. if (!sctx->cmd_head) {
  699. sctx->cmd_head = cctx;
  700. sctx->cmd_tail = cctx;
  701. } else {
  702. sctx->cmd_tail->cmd = cctx;
  703. sctx->cmd_tail = cctx;
  704. }
  705. return 0;
  706. }
  707. static int sb_build_command_load(struct sb_image_ctx *ictx,
  708. struct sb_cmd_list *cmd)
  709. {
  710. struct sb_section_ctx *sctx = ictx->sect_tail;
  711. struct sb_cmd_ctx *cctx;
  712. struct sb_command *ccmd;
  713. char *tok;
  714. int ret, is_ivt = 0, is_dcd = 0;
  715. uint32_t dest, dcd = 0;
  716. cctx = calloc(1, sizeof(*cctx));
  717. if (!cctx)
  718. return -ENOMEM;
  719. ccmd = &cctx->payload;
  720. /*
  721. * Prepare the command.
  722. */
  723. tok = strtok(cmd->cmd, " ");
  724. if (!tok) {
  725. fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
  726. cmd->lineno);
  727. ret = -EINVAL;
  728. goto err;
  729. }
  730. /* Check for "IVT" flag. */
  731. if (!strcmp(tok, "IVT"))
  732. is_ivt = 1;
  733. if (!strcmp(tok, "DCD"))
  734. is_dcd = 1;
  735. if (is_ivt || is_dcd) {
  736. tok = strtok(NULL, " ");
  737. if (!tok) {
  738. fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
  739. cmd->lineno);
  740. ret = -EINVAL;
  741. goto err;
  742. }
  743. }
  744. /* Read load destination address. */
  745. ret = sb_token_to_long(tok, &dest);
  746. if (ret) {
  747. fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
  748. cmd->lineno);
  749. goto err;
  750. }
  751. /* Read filename or IVT entrypoint or DCD block ID. */
  752. tok = strtok(NULL, " ");
  753. if (!tok) {
  754. fprintf(stderr,
  755. "#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n",
  756. cmd->lineno);
  757. ret = -EINVAL;
  758. goto err;
  759. }
  760. if (is_ivt) {
  761. /* Handle IVT. */
  762. struct sb_ivt_header *ivt;
  763. uint32_t ivtep;
  764. ret = sb_token_to_long(tok, &ivtep);
  765. if (ret) {
  766. fprintf(stderr,
  767. "#%i ERR: Incorrect IVT entry point!\n",
  768. cmd->lineno);
  769. goto err;
  770. }
  771. ivt = calloc(1, sizeof(*ivt));
  772. if (!ivt) {
  773. ret = -ENOMEM;
  774. goto err;
  775. }
  776. ivt->header = sb_hab_ivt_header();
  777. ivt->entry = ivtep;
  778. ivt->self = dest;
  779. cctx->data = (uint8_t *)ivt;
  780. cctx->length = sizeof(*ivt);
  781. } else if (is_dcd) {
  782. struct sb_dcd_ctx *dctx = ictx->dcd_head;
  783. uint32_t dcdid;
  784. uint8_t *payload;
  785. uint32_t asize;
  786. ret = sb_token_to_long(tok, &dcdid);
  787. if (ret) {
  788. fprintf(stderr,
  789. "#%i ERR: Incorrect DCD block ID!\n",
  790. cmd->lineno);
  791. goto err;
  792. }
  793. while (dctx) {
  794. if (dctx->id == dcdid)
  795. break;
  796. dctx = dctx->dcd;
  797. }
  798. if (!dctx) {
  799. fprintf(stderr, "#%i ERR: DCD block %08x not found!\n",
  800. cmd->lineno, dcdid);
  801. goto err;
  802. }
  803. asize = roundup(dctx->size, SB_BLOCK_SIZE);
  804. payload = calloc(1, asize);
  805. if (!payload) {
  806. ret = -ENOMEM;
  807. goto err;
  808. }
  809. memcpy(payload, dctx->payload, dctx->size);
  810. cctx->data = payload;
  811. cctx->length = asize;
  812. /* Set the Load DCD flag. */
  813. dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD;
  814. } else {
  815. /* Regular LOAD of a file. */
  816. ret = sb_load_file(cctx, tok);
  817. if (ret) {
  818. fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
  819. cmd->lineno, tok);
  820. goto err;
  821. }
  822. }
  823. if (cctx->length & (SB_BLOCK_SIZE - 1)) {
  824. fprintf(stderr, "#%i ERR: Unaligned payload!\n",
  825. cmd->lineno);
  826. }
  827. /*
  828. * Construct the command.
  829. */
  830. ccmd->header.checksum = 0x5a;
  831. ccmd->header.tag = ROM_LOAD_CMD;
  832. ccmd->header.flags = dcd;
  833. ccmd->load.address = dest;
  834. ccmd->load.count = cctx->length;
  835. ccmd->load.crc32 = pbl_crc32(0,
  836. (const char *)cctx->data,
  837. cctx->length);
  838. cctx->size = sizeof(*ccmd) + cctx->length;
  839. /*
  840. * Append the command to the last section.
  841. */
  842. if (!sctx->cmd_head) {
  843. sctx->cmd_head = cctx;
  844. sctx->cmd_tail = cctx;
  845. } else {
  846. sctx->cmd_tail->cmd = cctx;
  847. sctx->cmd_tail = cctx;
  848. }
  849. return 0;
  850. err:
  851. free(cctx);
  852. return ret;
  853. }
  854. static int sb_build_command_fill(struct sb_image_ctx *ictx,
  855. struct sb_cmd_list *cmd)
  856. {
  857. struct sb_section_ctx *sctx = ictx->sect_tail;
  858. struct sb_cmd_ctx *cctx;
  859. struct sb_command *ccmd;
  860. char *tok;
  861. uint32_t address, pattern, length;
  862. int ret;
  863. cctx = calloc(1, sizeof(*cctx));
  864. if (!cctx)
  865. return -ENOMEM;
  866. ccmd = &cctx->payload;
  867. /*
  868. * Prepare the command.
  869. */
  870. tok = strtok(cmd->cmd, " ");
  871. if (!tok) {
  872. fprintf(stderr, "#%i ERR: Missing FILL address!\n",
  873. cmd->lineno);
  874. ret = -EINVAL;
  875. goto err;
  876. }
  877. /* Read fill destination address. */
  878. ret = sb_token_to_long(tok, &address);
  879. if (ret) {
  880. fprintf(stderr, "#%i ERR: Incorrect FILL address!\n",
  881. cmd->lineno);
  882. goto err;
  883. }
  884. tok = strtok(NULL, " ");
  885. if (!tok) {
  886. fprintf(stderr, "#%i ERR: Missing FILL pattern!\n",
  887. cmd->lineno);
  888. ret = -EINVAL;
  889. goto err;
  890. }
  891. /* Read fill pattern address. */
  892. ret = sb_token_to_long(tok, &pattern);
  893. if (ret) {
  894. fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n",
  895. cmd->lineno);
  896. goto err;
  897. }
  898. tok = strtok(NULL, " ");
  899. if (!tok) {
  900. fprintf(stderr, "#%i ERR: Missing FILL length!\n",
  901. cmd->lineno);
  902. ret = -EINVAL;
  903. goto err;
  904. }
  905. /* Read fill pattern address. */
  906. ret = sb_token_to_long(tok, &length);
  907. if (ret) {
  908. fprintf(stderr, "#%i ERR: Incorrect FILL length!\n",
  909. cmd->lineno);
  910. goto err;
  911. }
  912. /*
  913. * Construct the command.
  914. */
  915. ccmd->header.checksum = 0x5a;
  916. ccmd->header.tag = ROM_FILL_CMD;
  917. ccmd->fill.address = address;
  918. ccmd->fill.count = length;
  919. ccmd->fill.pattern = pattern;
  920. cctx->size = sizeof(*ccmd);
  921. /*
  922. * Append the command to the last section.
  923. */
  924. if (!sctx->cmd_head) {
  925. sctx->cmd_head = cctx;
  926. sctx->cmd_tail = cctx;
  927. } else {
  928. sctx->cmd_tail->cmd = cctx;
  929. sctx->cmd_tail = cctx;
  930. }
  931. return 0;
  932. err:
  933. free(cctx);
  934. return ret;
  935. }
  936. static int sb_build_command_jump_call(struct sb_image_ctx *ictx,
  937. struct sb_cmd_list *cmd,
  938. unsigned int is_call)
  939. {
  940. struct sb_section_ctx *sctx = ictx->sect_tail;
  941. struct sb_cmd_ctx *cctx;
  942. struct sb_command *ccmd;
  943. char *tok;
  944. uint32_t dest, arg = 0x0;
  945. uint32_t hab = 0;
  946. int ret;
  947. const char *cmdname = is_call ? "CALL" : "JUMP";
  948. cctx = calloc(1, sizeof(*cctx));
  949. if (!cctx)
  950. return -ENOMEM;
  951. ccmd = &cctx->payload;
  952. /*
  953. * Prepare the command.
  954. */
  955. tok = strtok(cmd->cmd, " ");
  956. if (!tok) {
  957. fprintf(stderr,
  958. "#%i ERR: Missing %s address or 'HAB'!\n",
  959. cmd->lineno, cmdname);
  960. ret = -EINVAL;
  961. goto err;
  962. }
  963. /* Check for "HAB" flag. */
  964. if (!strcmp(tok, "HAB")) {
  965. hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB;
  966. tok = strtok(NULL, " ");
  967. if (!tok) {
  968. fprintf(stderr, "#%i ERR: Missing %s address!\n",
  969. cmd->lineno, cmdname);
  970. ret = -EINVAL;
  971. goto err;
  972. }
  973. }
  974. /* Read load destination address. */
  975. ret = sb_token_to_long(tok, &dest);
  976. if (ret) {
  977. fprintf(stderr, "#%i ERR: Incorrect %s address!\n",
  978. cmd->lineno, cmdname);
  979. goto err;
  980. }
  981. tok = strtok(NULL, " ");
  982. if (tok) {
  983. ret = sb_token_to_long(tok, &arg);
  984. if (ret) {
  985. fprintf(stderr,
  986. "#%i ERR: Incorrect %s argument!\n",
  987. cmd->lineno, cmdname);
  988. goto err;
  989. }
  990. }
  991. /*
  992. * Construct the command.
  993. */
  994. ccmd->header.checksum = 0x5a;
  995. ccmd->header.tag = is_call ? ROM_CALL_CMD : ROM_JUMP_CMD;
  996. ccmd->header.flags = hab;
  997. ccmd->call.address = dest;
  998. ccmd->call.argument = arg;
  999. cctx->size = sizeof(*ccmd);
  1000. /*
  1001. * Append the command to the last section.
  1002. */
  1003. if (!sctx->cmd_head) {
  1004. sctx->cmd_head = cctx;
  1005. sctx->cmd_tail = cctx;
  1006. } else {
  1007. sctx->cmd_tail->cmd = cctx;
  1008. sctx->cmd_tail = cctx;
  1009. }
  1010. return 0;
  1011. err:
  1012. free(cctx);
  1013. return ret;
  1014. }
  1015. static int sb_build_command_jump(struct sb_image_ctx *ictx,
  1016. struct sb_cmd_list *cmd)
  1017. {
  1018. return sb_build_command_jump_call(ictx, cmd, 0);
  1019. }
  1020. static int sb_build_command_call(struct sb_image_ctx *ictx,
  1021. struct sb_cmd_list *cmd)
  1022. {
  1023. return sb_build_command_jump_call(ictx, cmd, 1);
  1024. }
  1025. static int sb_build_command_mode(struct sb_image_ctx *ictx,
  1026. struct sb_cmd_list *cmd)
  1027. {
  1028. struct sb_section_ctx *sctx = ictx->sect_tail;
  1029. struct sb_cmd_ctx *cctx;
  1030. struct sb_command *ccmd;
  1031. char *tok;
  1032. int ret;
  1033. unsigned int i;
  1034. uint32_t mode = 0xffffffff;
  1035. cctx = calloc(1, sizeof(*cctx));
  1036. if (!cctx)
  1037. return -ENOMEM;
  1038. ccmd = &cctx->payload;
  1039. /*
  1040. * Prepare the command.
  1041. */
  1042. tok = strtok(cmd->cmd, " ");
  1043. if (!tok) {
  1044. fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n",
  1045. cmd->lineno);
  1046. ret = -EINVAL;
  1047. goto err;
  1048. }
  1049. for (i = 0; i < ARRAY_SIZE(modetable); i++) {
  1050. if (!strcmp(tok, modetable[i].name)) {
  1051. mode = modetable[i].mode;
  1052. break;
  1053. }
  1054. if (!modetable[i].altname)
  1055. continue;
  1056. if (!strcmp(tok, modetable[i].altname)) {
  1057. mode = modetable[i].mode;
  1058. break;
  1059. }
  1060. }
  1061. if (mode == 0xffffffff) {
  1062. fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n",
  1063. cmd->lineno);
  1064. ret = -EINVAL;
  1065. goto err;
  1066. }
  1067. /*
  1068. * Construct the command.
  1069. */
  1070. ccmd->header.checksum = 0x5a;
  1071. ccmd->header.tag = ROM_MODE_CMD;
  1072. ccmd->mode.mode = mode;
  1073. cctx->size = sizeof(*ccmd);
  1074. /*
  1075. * Append the command to the last section.
  1076. */
  1077. if (!sctx->cmd_head) {
  1078. sctx->cmd_head = cctx;
  1079. sctx->cmd_tail = cctx;
  1080. } else {
  1081. sctx->cmd_tail->cmd = cctx;
  1082. sctx->cmd_tail = cctx;
  1083. }
  1084. return 0;
  1085. err:
  1086. free(cctx);
  1087. return ret;
  1088. }
  1089. static int sb_prefill_image_header(struct sb_image_ctx *ictx)
  1090. {
  1091. struct sb_boot_image_header *hdr = &ictx->payload;
  1092. /* Fill signatures */
  1093. memcpy(hdr->signature1, "STMP", 4);
  1094. memcpy(hdr->signature2, "sgtl", 4);
  1095. /* SB Image version 1.1 */
  1096. hdr->major_version = SB_VERSION_MAJOR;
  1097. hdr->minor_version = SB_VERSION_MINOR;
  1098. /* Boot image major version */
  1099. hdr->product_version.major = htons(0x999);
  1100. hdr->product_version.minor = htons(0x999);
  1101. hdr->product_version.revision = htons(0x999);
  1102. /* Boot image major version */
  1103. hdr->component_version.major = htons(0x999);
  1104. hdr->component_version.minor = htons(0x999);
  1105. hdr->component_version.revision = htons(0x999);
  1106. /* Drive tag must be 0x0 for i.MX23 */
  1107. hdr->drive_tag = 0;
  1108. hdr->header_blocks =
  1109. sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
  1110. hdr->section_header_size =
  1111. sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
  1112. hdr->timestamp_us = sb_get_timestamp() * 1000000;
  1113. hdr->flags = ictx->display_progress ?
  1114. SB_IMAGE_FLAG_DISPLAY_PROGRESS : 0;
  1115. /* FIXME -- We support only default key */
  1116. hdr->key_count = 1;
  1117. return 0;
  1118. }
  1119. static int sb_postfill_image_header(struct sb_image_ctx *ictx)
  1120. {
  1121. struct sb_boot_image_header *hdr = &ictx->payload;
  1122. struct sb_section_ctx *sctx = ictx->sect_head;
  1123. uint32_t kd_size, sections_blocks;
  1124. EVP_MD_CTX *md_ctx;
  1125. /* The main SB header size in blocks. */
  1126. hdr->image_blocks = hdr->header_blocks;
  1127. /* Size of the key dictionary, which has single zero entry. */
  1128. kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key);
  1129. hdr->image_blocks += kd_size / SB_BLOCK_SIZE;
  1130. /* Now count the payloads. */
  1131. hdr->section_count = ictx->sect_count;
  1132. while (sctx) {
  1133. hdr->image_blocks += sctx->size / SB_BLOCK_SIZE;
  1134. sctx = sctx->sect;
  1135. }
  1136. if (!ictx->sect_boot_found) {
  1137. fprintf(stderr, "ERR: No bootable section selected!\n");
  1138. return -EINVAL;
  1139. }
  1140. hdr->first_boot_section_id = ictx->sect_boot;
  1141. /* The n * SB section size in blocks. */
  1142. sections_blocks = hdr->section_count * hdr->section_header_size;
  1143. hdr->image_blocks += sections_blocks;
  1144. /* Key dictionary offset. */
  1145. hdr->key_dictionary_block = hdr->header_blocks + sections_blocks;
  1146. /* Digest of the whole image. */
  1147. hdr->image_blocks += 2;
  1148. /* Pointer past the dictionary. */
  1149. hdr->first_boot_tag_block =
  1150. hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE;
  1151. /* Compute header digest. */
  1152. md_ctx = EVP_MD_CTX_new();
  1153. EVP_DigestInit(md_ctx, EVP_sha1());
  1154. EVP_DigestUpdate(md_ctx, hdr->signature1,
  1155. sizeof(struct sb_boot_image_header) -
  1156. sizeof(hdr->digest));
  1157. EVP_DigestFinal(md_ctx, hdr->digest, NULL);
  1158. EVP_MD_CTX_free(md_ctx);
  1159. return 0;
  1160. }
  1161. static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx)
  1162. {
  1163. /* Fixup the placement of sections. */
  1164. struct sb_boot_image_header *ihdr = &ictx->payload;
  1165. struct sb_section_ctx *sctx = ictx->sect_head;
  1166. struct sb_sections_header *shdr;
  1167. struct sb_cmd_ctx *cctx;
  1168. struct sb_command *ccmd;
  1169. uint32_t offset = ihdr->first_boot_tag_block;
  1170. while (sctx) {
  1171. shdr = &sctx->payload;
  1172. /* Fill in the section TAG offset. */
  1173. shdr->section_offset = offset + 1;
  1174. offset += shdr->section_size;
  1175. /* Section length is measured from the TAG block. */
  1176. shdr->section_size--;
  1177. /* Fixup the TAG command. */
  1178. cctx = sctx->cmd_head;
  1179. while (cctx) {
  1180. ccmd = &cctx->payload;
  1181. if (ccmd->header.tag == ROM_TAG_CMD) {
  1182. ccmd->tag.section_number = shdr->section_number;
  1183. ccmd->tag.section_length = shdr->section_size;
  1184. ccmd->tag.section_flags = shdr->section_flags;
  1185. }
  1186. /* Update the command checksum. */
  1187. ccmd->header.checksum = sb_command_checksum(ccmd);
  1188. cctx = cctx->cmd;
  1189. }
  1190. sctx = sctx->sect;
  1191. }
  1192. return 0;
  1193. }
  1194. static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  1195. {
  1196. char *tok;
  1197. char *line = cmd->cmd;
  1198. char *rptr = NULL;
  1199. int ret;
  1200. /* Analyze the identifier on this line first. */
  1201. tok = strtok_r(line, " ", &rptr);
  1202. if (!tok || (strlen(tok) == 0)) {
  1203. fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno);
  1204. return -EINVAL;
  1205. }
  1206. cmd->cmd = rptr;
  1207. /* set DISPLAY_PROGRESS flag */
  1208. if (!strcmp(tok, "DISPLAYPROGRESS")) {
  1209. ictx->display_progress = 1;
  1210. return 0;
  1211. }
  1212. /* DCD */
  1213. if (!strcmp(tok, "DCD")) {
  1214. ictx->in_section = 0;
  1215. ictx->in_dcd = 1;
  1216. sb_build_dcd(ictx, cmd);
  1217. return 0;
  1218. }
  1219. /* Section */
  1220. if (!strcmp(tok, "SECTION")) {
  1221. ictx->in_section = 1;
  1222. ictx->in_dcd = 0;
  1223. sb_build_section(ictx, cmd);
  1224. return 0;
  1225. }
  1226. if (!ictx->in_section && !ictx->in_dcd) {
  1227. fprintf(stderr, "#%i ERR: Data outside of a section!\n",
  1228. cmd->lineno);
  1229. return -EINVAL;
  1230. }
  1231. if (ictx->in_section) {
  1232. /* Section commands */
  1233. if (!strcmp(tok, "NOP")) {
  1234. ret = sb_build_command_nop(ictx);
  1235. } else if (!strcmp(tok, "TAG")) {
  1236. ret = sb_build_command_tag(ictx, cmd);
  1237. } else if (!strcmp(tok, "LOAD")) {
  1238. ret = sb_build_command_load(ictx, cmd);
  1239. } else if (!strcmp(tok, "FILL")) {
  1240. ret = sb_build_command_fill(ictx, cmd);
  1241. } else if (!strcmp(tok, "JUMP")) {
  1242. ret = sb_build_command_jump(ictx, cmd);
  1243. } else if (!strcmp(tok, "CALL")) {
  1244. ret = sb_build_command_call(ictx, cmd);
  1245. } else if (!strcmp(tok, "MODE")) {
  1246. ret = sb_build_command_mode(ictx, cmd);
  1247. } else {
  1248. fprintf(stderr,
  1249. "#%i ERR: Unsupported instruction '%s'!\n",
  1250. cmd->lineno, tok);
  1251. return -ENOTSUP;
  1252. }
  1253. } else if (ictx->in_dcd) {
  1254. char *lptr;
  1255. uint32_t ilen = '1';
  1256. tok = strtok_r(tok, ".", &lptr);
  1257. if (!tok || (strlen(tok) == 0) || (lptr && strlen(lptr) != 1)) {
  1258. fprintf(stderr, "#%i ERR: Invalid line!\n",
  1259. cmd->lineno);
  1260. return -EINVAL;
  1261. }
  1262. if (lptr &&
  1263. (lptr[0] != '1' && lptr[0] != '2' && lptr[0] != '4')) {
  1264. fprintf(stderr, "#%i ERR: Invalid instruction width!\n",
  1265. cmd->lineno);
  1266. return -EINVAL;
  1267. }
  1268. if (lptr)
  1269. ilen = lptr[0] - '1';
  1270. /* DCD commands */
  1271. if (!strcmp(tok, "WRITE")) {
  1272. ret = sb_build_dcd_block(ictx, cmd,
  1273. SB_DCD_WRITE | ilen);
  1274. } else if (!strcmp(tok, "ANDC")) {
  1275. ret = sb_build_dcd_block(ictx, cmd,
  1276. SB_DCD_ANDC | ilen);
  1277. } else if (!strcmp(tok, "ORR")) {
  1278. ret = sb_build_dcd_block(ictx, cmd,
  1279. SB_DCD_ORR | ilen);
  1280. } else if (!strcmp(tok, "EQZ")) {
  1281. ret = sb_build_dcd_block(ictx, cmd,
  1282. SB_DCD_CHK_EQZ | ilen);
  1283. } else if (!strcmp(tok, "EQ")) {
  1284. ret = sb_build_dcd_block(ictx, cmd,
  1285. SB_DCD_CHK_EQ | ilen);
  1286. } else if (!strcmp(tok, "NEQ")) {
  1287. ret = sb_build_dcd_block(ictx, cmd,
  1288. SB_DCD_CHK_NEQ | ilen);
  1289. } else if (!strcmp(tok, "NEZ")) {
  1290. ret = sb_build_dcd_block(ictx, cmd,
  1291. SB_DCD_CHK_NEZ | ilen);
  1292. } else if (!strcmp(tok, "NOOP")) {
  1293. ret = sb_build_dcd_block(ictx, cmd, SB_DCD_NOOP);
  1294. } else {
  1295. fprintf(stderr,
  1296. "#%i ERR: Unsupported instruction '%s'!\n",
  1297. cmd->lineno, tok);
  1298. return -ENOTSUP;
  1299. }
  1300. } else {
  1301. fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n",
  1302. cmd->lineno, tok);
  1303. return -ENOTSUP;
  1304. }
  1305. /*
  1306. * Here we have at least one section with one command, otherwise we
  1307. * would have failed already higher above.
  1308. *
  1309. * FIXME -- should the updating happen here ?
  1310. */
  1311. if (ictx->in_section && !ret) {
  1312. ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size;
  1313. ictx->sect_tail->payload.section_size =
  1314. ictx->sect_tail->size / SB_BLOCK_SIZE;
  1315. }
  1316. return ret;
  1317. }
  1318. static int sb_load_cmdfile(struct sb_image_ctx *ictx)
  1319. {
  1320. struct sb_cmd_list cmd;
  1321. int lineno = 1;
  1322. FILE *fp;
  1323. char *line = NULL;
  1324. ssize_t rlen;
  1325. size_t len;
  1326. fp = fopen(ictx->cfg_filename, "r");
  1327. if (!fp)
  1328. goto err_file;
  1329. while ((rlen = getline(&line, &len, fp)) > 0) {
  1330. memset(&cmd, 0, sizeof(cmd));
  1331. /* Strip the trailing newline. */
  1332. line[rlen - 1] = '\0';
  1333. cmd.cmd = line;
  1334. cmd.len = rlen;
  1335. cmd.lineno = lineno++;
  1336. sb_parse_line(ictx, &cmd);
  1337. }
  1338. free(line);
  1339. fclose(fp);
  1340. return 0;
  1341. err_file:
  1342. fclose(fp);
  1343. fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
  1344. ictx->cfg_filename);
  1345. return -EINVAL;
  1346. }
  1347. static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx)
  1348. {
  1349. int ret;
  1350. ret = sb_load_cmdfile(ictx);
  1351. if (ret)
  1352. return ret;
  1353. ret = sb_prefill_image_header(ictx);
  1354. if (ret)
  1355. return ret;
  1356. ret = sb_postfill_image_header(ictx);
  1357. if (ret)
  1358. return ret;
  1359. ret = sb_fixup_sections_and_tags(ictx);
  1360. if (ret)
  1361. return ret;
  1362. return 0;
  1363. }
  1364. static int sb_verify_image_header(struct sb_image_ctx *ictx,
  1365. FILE *fp, long fsize)
  1366. {
  1367. /* Verify static fields in the image header. */
  1368. struct sb_boot_image_header *hdr = &ictx->payload;
  1369. const char *stat[2] = { "[PASS]", "[FAIL]" };
  1370. struct tm tm;
  1371. int sz, ret = 0;
  1372. unsigned char digest[20];
  1373. EVP_MD_CTX *md_ctx;
  1374. unsigned long size;
  1375. /* Start image-wide crypto. */
  1376. ictx->md_ctx = EVP_MD_CTX_new();
  1377. EVP_DigestInit(ictx->md_ctx, EVP_sha1());
  1378. soprintf(ictx, "---------- Verifying SB Image Header ----------\n");
  1379. size = fread(&ictx->payload, 1, sizeof(ictx->payload), fp);
  1380. if (size != sizeof(ictx->payload)) {
  1381. fprintf(stderr, "ERR: SB image header too short!\n");
  1382. return -EINVAL;
  1383. }
  1384. /* Compute header digest. */
  1385. md_ctx = EVP_MD_CTX_new();
  1386. EVP_DigestInit(md_ctx, EVP_sha1());
  1387. EVP_DigestUpdate(md_ctx, hdr->signature1,
  1388. sizeof(struct sb_boot_image_header) -
  1389. sizeof(hdr->digest));
  1390. EVP_DigestFinal(md_ctx, digest, NULL);
  1391. EVP_MD_CTX_free(md_ctx);
  1392. sb_aes_init(ictx, NULL, 1);
  1393. sb_encrypt_sb_header(ictx);
  1394. if (memcmp(digest, hdr->digest, 20))
  1395. ret = -EINVAL;
  1396. soprintf(ictx, "%s Image header checksum: %s\n", stat[!!ret],
  1397. ret ? "BAD" : "OK");
  1398. if (ret)
  1399. return ret;
  1400. if (memcmp(hdr->signature1, "STMP", 4) ||
  1401. memcmp(hdr->signature2, "sgtl", 4))
  1402. ret = -EINVAL;
  1403. soprintf(ictx, "%s Signatures: '%.4s' '%.4s'\n",
  1404. stat[!!ret], hdr->signature1, hdr->signature2);
  1405. if (ret)
  1406. return ret;
  1407. if ((hdr->major_version != SB_VERSION_MAJOR) ||
  1408. ((hdr->minor_version != 1) && (hdr->minor_version != 2)))
  1409. ret = -EINVAL;
  1410. soprintf(ictx, "%s Image version: v%i.%i\n", stat[!!ret],
  1411. hdr->major_version, hdr->minor_version);
  1412. if (ret)
  1413. return ret;
  1414. ret = sb_get_time(hdr->timestamp_us / 1000000, &tm);
  1415. soprintf(ictx,
  1416. "%s Creation time: %02i:%02i:%02i %02i/%02i/%04i\n",
  1417. stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec,
  1418. tm.tm_mday, tm.tm_mon, tm.tm_year + 2000);
  1419. if (ret)
  1420. return ret;
  1421. soprintf(ictx, "%s Product version: %x.%x.%x\n", stat[0],
  1422. ntohs(hdr->product_version.major),
  1423. ntohs(hdr->product_version.minor),
  1424. ntohs(hdr->product_version.revision));
  1425. soprintf(ictx, "%s Component version: %x.%x.%x\n", stat[0],
  1426. ntohs(hdr->component_version.major),
  1427. ntohs(hdr->component_version.minor),
  1428. ntohs(hdr->component_version.revision));
  1429. if (hdr->flags & ~SB_IMAGE_FLAGS_MASK)
  1430. ret = -EINVAL;
  1431. soprintf(ictx, "%s Image flags: %s\n", stat[!!ret],
  1432. hdr->flags & SB_IMAGE_FLAG_DISPLAY_PROGRESS ?
  1433. "Display_progress" : "");
  1434. if (ret)
  1435. return ret;
  1436. if (hdr->drive_tag != 0)
  1437. ret = -EINVAL;
  1438. soprintf(ictx, "%s Drive tag: %i\n", stat[!!ret],
  1439. hdr->drive_tag);
  1440. if (ret)
  1441. return ret;
  1442. sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
  1443. if (hdr->header_blocks != sz)
  1444. ret = -EINVAL;
  1445. soprintf(ictx, "%s Image header size (blocks): %i\n", stat[!!ret],
  1446. hdr->header_blocks);
  1447. if (ret)
  1448. return ret;
  1449. sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
  1450. if (hdr->section_header_size != sz)
  1451. ret = -EINVAL;
  1452. soprintf(ictx, "%s Section header size (blocks): %i\n", stat[!!ret],
  1453. hdr->section_header_size);
  1454. if (ret)
  1455. return ret;
  1456. soprintf(ictx, "%s Sections count: %i\n", stat[!!ret],
  1457. hdr->section_count);
  1458. soprintf(ictx, "%s First bootable section %i\n", stat[!!ret],
  1459. hdr->first_boot_section_id);
  1460. if (hdr->image_blocks != fsize / SB_BLOCK_SIZE)
  1461. ret = -EINVAL;
  1462. soprintf(ictx, "%s Image size (blocks): %i\n", stat[!!ret],
  1463. hdr->image_blocks);
  1464. if (ret)
  1465. return ret;
  1466. sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
  1467. if (hdr->key_dictionary_block != sz)
  1468. ret = -EINVAL;
  1469. soprintf(ictx, "%s Key dict offset (blocks): %i\n", stat[!!ret],
  1470. hdr->key_dictionary_block);
  1471. if (ret)
  1472. return ret;
  1473. if (hdr->key_count != 1)
  1474. ret = -EINVAL;
  1475. soprintf(ictx, "%s Number of encryption keys: %i\n", stat[!!ret],
  1476. hdr->key_count);
  1477. if (ret)
  1478. return ret;
  1479. sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
  1480. sz += hdr->key_count *
  1481. sizeof(struct sb_key_dictionary_key) / SB_BLOCK_SIZE;
  1482. if (hdr->first_boot_tag_block != (unsigned)sz)
  1483. ret = -EINVAL;
  1484. soprintf(ictx, "%s First TAG block (blocks): %i\n", stat[!!ret],
  1485. hdr->first_boot_tag_block);
  1486. if (ret)
  1487. return ret;
  1488. return 0;
  1489. }
  1490. static void sb_decrypt_tag(struct sb_image_ctx *ictx,
  1491. struct sb_cmd_ctx *cctx)
  1492. {
  1493. EVP_MD_CTX *md_ctx = ictx->md_ctx;
  1494. struct sb_command *cmd = &cctx->payload;
  1495. sb_aes_crypt(ictx, (uint8_t *)&cctx->c_payload,
  1496. (uint8_t *)&cctx->payload, sizeof(*cmd));
  1497. EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
  1498. }
  1499. static int sb_verify_command(struct sb_image_ctx *ictx,
  1500. struct sb_cmd_ctx *cctx, FILE *fp,
  1501. unsigned long *tsize)
  1502. {
  1503. struct sb_command *ccmd = &cctx->payload;
  1504. unsigned long size, asize;
  1505. char *csum, *flag = "";
  1506. int ret;
  1507. unsigned int i;
  1508. uint8_t csn, csc = ccmd->header.checksum;
  1509. ccmd->header.checksum = 0x5a;
  1510. csn = sb_command_checksum(ccmd);
  1511. ccmd->header.checksum = csc;
  1512. if (csc == csn)
  1513. ret = 0;
  1514. else
  1515. ret = -EINVAL;
  1516. csum = ret ? "checksum BAD" : "checksum OK";
  1517. switch (ccmd->header.tag) {
  1518. case ROM_NOP_CMD:
  1519. soprintf(ictx, " NOOP # %s\n", csum);
  1520. return ret;
  1521. case ROM_TAG_CMD:
  1522. if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG)
  1523. flag = "LAST";
  1524. soprintf(ictx, " TAG %s # %s\n", flag, csum);
  1525. sb_aes_reinit(ictx, 0);
  1526. return ret;
  1527. case ROM_LOAD_CMD:
  1528. soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n",
  1529. ccmd->load.address, ccmd->load.count, csum);
  1530. cctx->length = ccmd->load.count;
  1531. asize = roundup(cctx->length, SB_BLOCK_SIZE);
  1532. cctx->data = malloc(asize);
  1533. if (!cctx->data)
  1534. return -ENOMEM;
  1535. size = fread(cctx->data, 1, asize, fp);
  1536. if (size != asize) {
  1537. fprintf(stderr,
  1538. "ERR: SB LOAD command payload too short!\n");
  1539. return -EINVAL;
  1540. }
  1541. *tsize += size;
  1542. EVP_DigestUpdate(ictx->md_ctx, cctx->data, asize);
  1543. sb_aes_crypt(ictx, cctx->data, cctx->data, asize);
  1544. if (ccmd->load.crc32 != pbl_crc32(0,
  1545. (const char *)cctx->data,
  1546. asize)) {
  1547. fprintf(stderr,
  1548. "ERR: SB LOAD command payload CRC32 invalid!\n");
  1549. return -EINVAL;
  1550. }
  1551. return 0;
  1552. case ROM_FILL_CMD:
  1553. soprintf(ictx,
  1554. " FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n",
  1555. ccmd->fill.address, ccmd->fill.count,
  1556. ccmd->fill.pattern, csum);
  1557. return 0;
  1558. case ROM_JUMP_CMD:
  1559. if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB)
  1560. flag = " HAB";
  1561. soprintf(ictx,
  1562. " JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n",
  1563. flag, ccmd->fill.address, ccmd->jump.argument, csum);
  1564. return 0;
  1565. case ROM_CALL_CMD:
  1566. if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB)
  1567. flag = " HAB";
  1568. soprintf(ictx,
  1569. " CALL%s addr=0x%08x r0_arg=0x%08x # %s\n",
  1570. flag, ccmd->fill.address, ccmd->jump.argument, csum);
  1571. return 0;
  1572. case ROM_MODE_CMD:
  1573. for (i = 0; i < ARRAY_SIZE(modetable); i++) {
  1574. if (ccmd->mode.mode == modetable[i].mode) {
  1575. soprintf(ictx, " MODE %s # %s\n",
  1576. modetable[i].name, csum);
  1577. break;
  1578. }
  1579. }
  1580. fprintf(stderr, " MODE !INVALID! # %s\n", csum);
  1581. return 0;
  1582. }
  1583. return ret;
  1584. }
  1585. static int sb_verify_commands(struct sb_image_ctx *ictx,
  1586. struct sb_section_ctx *sctx, FILE *fp)
  1587. {
  1588. unsigned long size, tsize = 0;
  1589. struct sb_cmd_ctx *cctx;
  1590. int ret;
  1591. sb_aes_reinit(ictx, 0);
  1592. while (tsize < sctx->size) {
  1593. cctx = calloc(1, sizeof(*cctx));
  1594. if (!cctx)
  1595. return -ENOMEM;
  1596. if (!sctx->cmd_head) {
  1597. sctx->cmd_head = cctx;
  1598. sctx->cmd_tail = cctx;
  1599. } else {
  1600. sctx->cmd_tail->cmd = cctx;
  1601. sctx->cmd_tail = cctx;
  1602. }
  1603. size = fread(&cctx->c_payload, 1, sizeof(cctx->c_payload), fp);
  1604. if (size != sizeof(cctx->c_payload)) {
  1605. fprintf(stderr, "ERR: SB command header too short!\n");
  1606. return -EINVAL;
  1607. }
  1608. tsize += size;
  1609. sb_decrypt_tag(ictx, cctx);
  1610. ret = sb_verify_command(ictx, cctx, fp, &tsize);
  1611. if (ret)
  1612. return -EINVAL;
  1613. }
  1614. return 0;
  1615. }
  1616. static int sb_verify_sections_cmds(struct sb_image_ctx *ictx, FILE *fp)
  1617. {
  1618. struct sb_boot_image_header *hdr = &ictx->payload;
  1619. struct sb_sections_header *shdr;
  1620. unsigned int i;
  1621. int ret;
  1622. struct sb_section_ctx *sctx;
  1623. unsigned long size;
  1624. char *bootable = "";
  1625. soprintf(ictx, "----- Verifying SB Sections and Commands -----\n");
  1626. for (i = 0; i < hdr->section_count; i++) {
  1627. sctx = calloc(1, sizeof(*sctx));
  1628. if (!sctx)
  1629. return -ENOMEM;
  1630. if (!ictx->sect_head) {
  1631. ictx->sect_head = sctx;
  1632. ictx->sect_tail = sctx;
  1633. } else {
  1634. ictx->sect_tail->sect = sctx;
  1635. ictx->sect_tail = sctx;
  1636. }
  1637. size = fread(&sctx->payload, 1, sizeof(sctx->payload), fp);
  1638. if (size != sizeof(sctx->payload)) {
  1639. fprintf(stderr, "ERR: SB section header too short!\n");
  1640. return -EINVAL;
  1641. }
  1642. }
  1643. size = fread(&ictx->sb_dict_key, 1, sizeof(ictx->sb_dict_key), fp);
  1644. if (size != sizeof(ictx->sb_dict_key)) {
  1645. fprintf(stderr, "ERR: SB key dictionary too short!\n");
  1646. return -EINVAL;
  1647. }
  1648. sb_encrypt_sb_sections_header(ictx);
  1649. sb_aes_reinit(ictx, 0);
  1650. sb_decrypt_key_dictionary_key(ictx);
  1651. sb_aes_reinit(ictx, 0);
  1652. sctx = ictx->sect_head;
  1653. while (sctx) {
  1654. shdr = &sctx->payload;
  1655. if (shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) {
  1656. sctx->boot = 1;
  1657. bootable = " BOOTABLE";
  1658. }
  1659. sctx->size = (shdr->section_size * SB_BLOCK_SIZE) +
  1660. sizeof(struct sb_command);
  1661. soprintf(ictx, "SECTION 0x%x%s # size = %i bytes\n",
  1662. shdr->section_number, bootable, sctx->size);
  1663. if (shdr->section_flags & ~SB_SECTION_FLAG_BOOTABLE)
  1664. fprintf(stderr, " WARN: Unknown section flag(s) %08x\n",
  1665. shdr->section_flags);
  1666. if ((shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) &&
  1667. (hdr->first_boot_section_id != shdr->section_number)) {
  1668. fprintf(stderr,
  1669. " WARN: Bootable section does ID not match image header ID!\n");
  1670. }
  1671. ret = sb_verify_commands(ictx, sctx, fp);
  1672. if (ret)
  1673. return ret;
  1674. sctx = sctx->sect;
  1675. }
  1676. /*
  1677. * FIXME IDEA:
  1678. * check if the first TAG command is at sctx->section_offset
  1679. */
  1680. return 0;
  1681. }
  1682. static int sb_verify_image_end(struct sb_image_ctx *ictx,
  1683. FILE *fp, off_t filesz)
  1684. {
  1685. uint8_t digest[32];
  1686. unsigned long size;
  1687. off_t pos;
  1688. int ret;
  1689. soprintf(ictx, "------------- Verifying image end -------------\n");
  1690. size = fread(digest, 1, sizeof(digest), fp);
  1691. if (size != sizeof(digest)) {
  1692. fprintf(stderr, "ERR: SB key dictionary too short!\n");
  1693. return -EINVAL;
  1694. }
  1695. pos = ftell(fp);
  1696. if (pos != filesz) {
  1697. fprintf(stderr, "ERR: Trailing data past the image!\n");
  1698. return -EINVAL;
  1699. }
  1700. /* Check the image digest. */
  1701. EVP_DigestFinal(ictx->md_ctx, ictx->digest, NULL);
  1702. EVP_MD_CTX_free(ictx->md_ctx);
  1703. /* Decrypt the image digest from the input image. */
  1704. sb_aes_reinit(ictx, 0);
  1705. sb_aes_crypt(ictx, digest, digest, sizeof(digest));
  1706. /* Check all of 20 bytes of the SHA1 hash. */
  1707. ret = memcmp(digest, ictx->digest, 20) ? -EINVAL : 0;
  1708. if (ret)
  1709. soprintf(ictx, "[FAIL] Full-image checksum: BAD\n");
  1710. else
  1711. soprintf(ictx, "[PASS] Full-image checksum: OK\n");
  1712. return ret;
  1713. }
  1714. static int sb_build_tree_from_img(struct sb_image_ctx *ictx)
  1715. {
  1716. long filesize;
  1717. int ret;
  1718. FILE *fp;
  1719. if (!ictx->input_filename) {
  1720. fprintf(stderr, "ERR: Missing filename!\n");
  1721. return -EINVAL;
  1722. }
  1723. fp = fopen(ictx->input_filename, "r");
  1724. if (!fp)
  1725. goto err_open;
  1726. ret = fseek(fp, 0, SEEK_END);
  1727. if (ret < 0)
  1728. goto err_file;
  1729. filesize = ftell(fp);
  1730. if (filesize < 0)
  1731. goto err_file;
  1732. ret = fseek(fp, 0, SEEK_SET);
  1733. if (ret < 0)
  1734. goto err_file;
  1735. if (filesize < (signed)sizeof(ictx->payload)) {
  1736. fprintf(stderr, "ERR: File too short!\n");
  1737. goto err_file;
  1738. }
  1739. if (filesize & (SB_BLOCK_SIZE - 1)) {
  1740. fprintf(stderr, "ERR: The file is not aligned!\n");
  1741. goto err_file;
  1742. }
  1743. /* Load and verify image header */
  1744. ret = sb_verify_image_header(ictx, fp, filesize);
  1745. if (ret)
  1746. goto err_verify;
  1747. /* Load and verify sections and commands */
  1748. ret = sb_verify_sections_cmds(ictx, fp);
  1749. if (ret)
  1750. goto err_verify;
  1751. ret = sb_verify_image_end(ictx, fp, filesize);
  1752. if (ret)
  1753. goto err_verify;
  1754. ret = 0;
  1755. err_verify:
  1756. soprintf(ictx, "-------------------- Result -------------------\n");
  1757. soprintf(ictx, "Verification %s\n", ret ? "FAILED" : "PASSED");
  1758. /* Stop the encryption session. */
  1759. sb_aes_deinit(ictx->cipher_ctx);
  1760. fclose(fp);
  1761. return ret;
  1762. err_file:
  1763. fclose(fp);
  1764. err_open:
  1765. fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
  1766. ictx->input_filename);
  1767. return -EINVAL;
  1768. }
  1769. static void sb_free_image(struct sb_image_ctx *ictx)
  1770. {
  1771. struct sb_section_ctx *sctx = ictx->sect_head, *s_head;
  1772. struct sb_dcd_ctx *dctx = ictx->dcd_head, *d_head;
  1773. struct sb_cmd_ctx *cctx, *c_head;
  1774. while (sctx) {
  1775. s_head = sctx;
  1776. c_head = sctx->cmd_head;
  1777. while (c_head) {
  1778. cctx = c_head;
  1779. c_head = c_head->cmd;
  1780. if (cctx->data)
  1781. free(cctx->data);
  1782. free(cctx);
  1783. }
  1784. sctx = sctx->sect;
  1785. free(s_head);
  1786. }
  1787. while (dctx) {
  1788. d_head = dctx;
  1789. dctx = dctx->dcd;
  1790. free(d_head->payload);
  1791. free(d_head);
  1792. }
  1793. }
  1794. /*
  1795. * MXSSB-MKIMAGE glue code.
  1796. */
  1797. static int mxsimage_check_image_types(uint8_t type)
  1798. {
  1799. if (type == IH_TYPE_MXSIMAGE)
  1800. return EXIT_SUCCESS;
  1801. else
  1802. return EXIT_FAILURE;
  1803. }
  1804. static void mxsimage_set_header(void *ptr, struct stat *sbuf, int ifd,
  1805. struct image_tool_params *params)
  1806. {
  1807. }
  1808. int mxsimage_check_params(struct image_tool_params *params)
  1809. {
  1810. if (!params)
  1811. return -1;
  1812. if (!strlen(params->imagename)) {
  1813. fprintf(stderr,
  1814. "Error: %s - Configuration file not specified, it is needed for mxsimage generation\n",
  1815. params->cmdname);
  1816. return -1;
  1817. }
  1818. /*
  1819. * Check parameters:
  1820. * XIP is not allowed and verify that incompatible
  1821. * parameters are not sent at the same time
  1822. * For example, if list is required a data image must not be provided
  1823. */
  1824. return (params->dflag && (params->fflag || params->lflag)) ||
  1825. (params->fflag && (params->dflag || params->lflag)) ||
  1826. (params->lflag && (params->dflag || params->fflag)) ||
  1827. (params->xflag) || !(strlen(params->imagename));
  1828. }
  1829. static int mxsimage_verify_print_header(char *file, int silent)
  1830. {
  1831. int ret;
  1832. struct sb_image_ctx ctx;
  1833. memset(&ctx, 0, sizeof(ctx));
  1834. ctx.input_filename = file;
  1835. ctx.silent_dump = silent;
  1836. ret = sb_build_tree_from_img(&ctx);
  1837. sb_free_image(&ctx);
  1838. return ret;
  1839. }
  1840. char *imagefile;
  1841. static int mxsimage_verify_header(unsigned char *ptr, int image_size,
  1842. struct image_tool_params *params)
  1843. {
  1844. struct sb_boot_image_header *hdr;
  1845. if (!ptr)
  1846. return -EINVAL;
  1847. hdr = (struct sb_boot_image_header *)ptr;
  1848. /*
  1849. * Check if the header contains the MXS image signatures,
  1850. * if so, do a full-image verification.
  1851. */
  1852. if (memcmp(hdr->signature1, "STMP", 4) ||
  1853. memcmp(hdr->signature2, "sgtl", 4))
  1854. return -EINVAL;
  1855. imagefile = params->imagefile;
  1856. return mxsimage_verify_print_header(params->imagefile, 1);
  1857. }
  1858. static void mxsimage_print_header(const void *hdr)
  1859. {
  1860. if (imagefile)
  1861. mxsimage_verify_print_header(imagefile, 0);
  1862. }
  1863. static int sb_build_image(struct sb_image_ctx *ictx,
  1864. struct image_type_params *tparams)
  1865. {
  1866. struct sb_boot_image_header *sb_header = &ictx->payload;
  1867. struct sb_section_ctx *sctx;
  1868. struct sb_cmd_ctx *cctx;
  1869. struct sb_command *ccmd;
  1870. struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
  1871. uint8_t *image, *iptr;
  1872. /* Calculate image size. */
  1873. uint32_t size = sizeof(*sb_header) +
  1874. ictx->sect_count * sizeof(struct sb_sections_header) +
  1875. sizeof(*sb_dict_key) + sizeof(ictx->digest);
  1876. sctx = ictx->sect_head;
  1877. while (sctx) {
  1878. size += sctx->size;
  1879. sctx = sctx->sect;
  1880. };
  1881. image = malloc(size);
  1882. if (!image)
  1883. return -ENOMEM;
  1884. iptr = image;
  1885. memcpy(iptr, sb_header, sizeof(*sb_header));
  1886. iptr += sizeof(*sb_header);
  1887. sctx = ictx->sect_head;
  1888. while (sctx) {
  1889. memcpy(iptr, &sctx->payload, sizeof(struct sb_sections_header));
  1890. iptr += sizeof(struct sb_sections_header);
  1891. sctx = sctx->sect;
  1892. };
  1893. memcpy(iptr, sb_dict_key, sizeof(*sb_dict_key));
  1894. iptr += sizeof(*sb_dict_key);
  1895. sctx = ictx->sect_head;
  1896. while (sctx) {
  1897. cctx = sctx->cmd_head;
  1898. while (cctx) {
  1899. ccmd = &cctx->payload;
  1900. memcpy(iptr, &cctx->c_payload, sizeof(cctx->payload));
  1901. iptr += sizeof(cctx->payload);
  1902. if (ccmd->header.tag == ROM_LOAD_CMD) {
  1903. memcpy(iptr, cctx->data, cctx->length);
  1904. iptr += cctx->length;
  1905. }
  1906. cctx = cctx->cmd;
  1907. }
  1908. sctx = sctx->sect;
  1909. };
  1910. memcpy(iptr, ictx->digest, sizeof(ictx->digest));
  1911. iptr += sizeof(ictx->digest);
  1912. /* Configure the mkimage */
  1913. tparams->hdr = image;
  1914. tparams->header_size = size;
  1915. return 0;
  1916. }
  1917. static int mxsimage_generate(struct image_tool_params *params,
  1918. struct image_type_params *tparams)
  1919. {
  1920. int ret;
  1921. struct sb_image_ctx ctx;
  1922. /* Do not copy the U-Boot image! */
  1923. params->skipcpy = 1;
  1924. memset(&ctx, 0, sizeof(ctx));
  1925. ctx.cfg_filename = params->imagename;
  1926. ctx.output_filename = params->imagefile;
  1927. ret = sb_build_tree_from_cfg(&ctx);
  1928. if (ret)
  1929. goto fail;
  1930. ret = sb_encrypt_image(&ctx);
  1931. if (!ret)
  1932. ret = sb_build_image(&ctx, tparams);
  1933. fail:
  1934. sb_free_image(&ctx);
  1935. return ret;
  1936. }
  1937. /*
  1938. * mxsimage parameters
  1939. */
  1940. U_BOOT_IMAGE_TYPE(
  1941. mxsimage,
  1942. "Freescale MXS Boot Image support",
  1943. 0,
  1944. NULL,
  1945. mxsimage_check_params,
  1946. mxsimage_verify_header,
  1947. mxsimage_print_header,
  1948. mxsimage_set_header,
  1949. NULL,
  1950. mxsimage_check_image_types,
  1951. NULL,
  1952. mxsimage_generate
  1953. );
  1954. #endif